From e328394184c1c0447c75e4447c66c35077067fa3 Mon Sep 17 00:00:00 2001 From: wangdequan Date: Sun, 7 Jun 2026 07:39:26 +0800 Subject: [PATCH] =?UTF-8?q?=E6=8C=89=E5=BB=BA=E8=AE=AE=EF=BC=8C=E7=BB=A7?= =?UTF-8?q?=E7=BB=AD=E4=B8=8B=E4=B8=80=E6=AD=A5=E5=B7=A5=E4=BD=9C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 结论：已将 LinuxCNC TP 源码纳入 wasm-port 的可复现 vendor 清单和 native probe 构建，新增真实 tpCreate/tpAddLine/tpRunCycle 线性运动验证，并通过 native probes 全量验证。 --- .../linuxcnc_wrap/linuxcnc_tp_api_probe.cpp | 146 + wasm-port/runtime/core/shims/hal.h | 7 + wasm-port/runtime/core/shims/rtapi.h | 4 + .../tests/native/verify_native_probes.sh | 14 + wasm-port/tools/build_native_probes.sh | 71 + wasm-port/tools/source-manifest.txt | 66 + .../linuxcnc/src/emc/kinematics/cubic.h | 62 + .../linuxcnc/src/emc/kinematics/kinematics.h | 215 + .../vendor/linuxcnc/src/emc/motion/axis.h | 60 + .../vendor/linuxcnc/src/emc/motion/mot_priv.h | 355 ++ .../vendor/linuxcnc/src/emc/motion/motion.h | 780 +++ .../linuxcnc/src/emc/motion/simple_tp.h | 107 + .../linuxcnc/src/emc/nml_intf/emcpose.c | 304 ++ .../linuxcnc/src/emc/nml_intf/motion_types.h | 25 + .../vendor/linuxcnc/src/emc/tp/blendmath.c | 1871 +++++++ .../vendor/linuxcnc/src/emc/tp/blendmath.h | 281 ++ .../linuxcnc/src/emc/tp/cruckig/block.c | 138 + .../linuxcnc/src/emc/tp/cruckig/block.h | 43 + .../linuxcnc/src/emc/tp/cruckig/brake.c | 201 + .../linuxcnc/src/emc/tp/cruckig/brake.h | 38 + .../linuxcnc/src/emc/tp/cruckig/calculator.c | 950 ++++ .../linuxcnc/src/emc/tp/cruckig/calculator.h | 71 + .../linuxcnc/src/emc/tp/cruckig/cruckig.c | 173 + .../linuxcnc/src/emc/tp/cruckig/cruckig.h | 54 + .../src/emc/tp/cruckig/cruckig_internal.h | 55 + .../src/emc/tp/cruckig/input_parameter.c | 408 ++ .../src/emc/tp/cruckig/input_parameter.h | 94 + .../src/emc/tp/cruckig/output_parameter.c | 104 + .../src/emc/tp/cruckig/output_parameter.h | 37 + .../linuxcnc/src/emc/tp/cruckig/position.h | 103 + .../src/emc/tp/cruckig/position_first_step1.c | 41 + .../src/emc/tp/cruckig/position_first_step2.c | 37 + .../emc/tp/cruckig/position_second_step1.c | 179 + .../emc/tp/cruckig/position_second_step2.c | 146 + .../src/emc/tp/cruckig/position_third_step1.c | 705 +++ .../src/emc/tp/cruckig/position_third_step2.c | 1370 +++++ .../linuxcnc/src/emc/tp/cruckig/profile.c | 539 ++ .../linuxcnc/src/emc/tp/cruckig/profile.h | 126 + .../linuxcnc/src/emc/tp/cruckig/result.h | 40 + .../linuxcnc/src/emc/tp/cruckig/roots.c | 408 ++ .../linuxcnc/src/emc/tp/cruckig/roots.h | 57 + .../linuxcnc/src/emc/tp/cruckig/trajectory.c | 315 ++ .../linuxcnc/src/emc/tp/cruckig/trajectory.h | 71 + .../linuxcnc/src/emc/tp/cruckig/utils.h | 26 + .../linuxcnc/src/emc/tp/cruckig/velocity.h | 63 + .../emc/tp/cruckig/velocity_second_step1.c | 40 + .../emc/tp/cruckig/velocity_second_step2.c | 39 + .../src/emc/tp/cruckig/velocity_third_step1.c | 187 + .../src/emc/tp/cruckig/velocity_third_step2.c | 146 + .../linuxcnc/src/emc/tp/ruckig_wrapper.c | 680 +++ .../linuxcnc/src/emc/tp/ruckig_wrapper.h | 221 + .../vendor/linuxcnc/src/emc/tp/sp_scurve.c | 653 +++ .../vendor/linuxcnc/src/emc/tp/sp_scurve.h | 60 + .../linuxcnc/src/emc/tp/spherical_arc.c | 202 + .../linuxcnc/src/emc/tp/spherical_arc.h | 67 + wasm-port/vendor/linuxcnc/src/emc/tp/tc.c | 1109 +++++ wasm-port/vendor/linuxcnc/src/emc/tp/tc.h | 128 + .../vendor/linuxcnc/src/emc/tp/tc_types.h | 215 + wasm-port/vendor/linuxcnc/src/emc/tp/tcq.c | 355 ++ wasm-port/vendor/linuxcnc/src/emc/tp/tcq.h | 75 + wasm-port/vendor/linuxcnc/src/emc/tp/tp.c | 4388 +++++++++++++++++ wasm-port/vendor/linuxcnc/src/emc/tp/tp.h | 99 + .../vendor/linuxcnc/src/emc/tp/tp_debug.h | 65 + .../vendor/linuxcnc/src/emc/tp/tp_types.h | 154 + .../linuxcnc/src/libnml/posemath/_posemath.c | 2005 ++++++++ .../linuxcnc/src/libnml/posemath/gomath.c | 3743 ++++++++++++++ .../linuxcnc/src/libnml/posemath/posemath.cc | 1411 ++++++ .../linuxcnc/src/libnml/posemath/sincos.c | 27 + .../vendor/linuxcnc/src/rtapi/rtapi_atomic.h | 81 + .../vendor/linuxcnc/src/rtapi/rtapi_bool.h | 25 + .../vendor/linuxcnc/src/rtapi/rtapi_limits.h | 27 + .../vendor/linuxcnc/src/rtapi/rtapi_slab.h | 38 + 72 files changed, 27200 insertions(+) create mode 100644 wasm-port/runtime/core/linuxcnc_wrap/linuxcnc_tp_api_probe.cpp create mode 100644 wasm-port/vendor/linuxcnc/src/emc/kinematics/cubic.h create mode 100644 wasm-port/vendor/linuxcnc/src/emc/kinematics/kinematics.h create mode 100644 wasm-port/vendor/linuxcnc/src/emc/motion/axis.h create mode 100644 wasm-port/vendor/linuxcnc/src/emc/motion/mot_priv.h create mode 100644 wasm-port/vendor/linuxcnc/src/emc/motion/motion.h create mode 100644 wasm-port/vendor/linuxcnc/src/emc/motion/simple_tp.h create mode 100644 wasm-port/vendor/linuxcnc/src/emc/nml_intf/emcpose.c create mode 100644 wasm-port/vendor/linuxcnc/src/emc/nml_intf/motion_types.h create mode 100644 wasm-port/vendor/linuxcnc/src/emc/tp/blendmath.c create mode 100644 wasm-port/vendor/linuxcnc/src/emc/tp/blendmath.h create mode 100644 wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/block.c create mode 100644 wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/block.h create mode 100644 wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/brake.c create mode 100644 wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/brake.h create mode 100644 wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/calculator.c create mode 100644 wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/calculator.h create mode 100644 wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/cruckig.c create mode 100644 wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/cruckig.h create mode 100644 wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/cruckig_internal.h create mode 100644 wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/input_parameter.c create mode 100644 wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/input_parameter.h create mode 100644 wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/output_parameter.c create mode 100644 wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/output_parameter.h create mode 100644 wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/position.h create mode 100644 wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/position_first_step1.c create mode 100644 wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/position_first_step2.c create mode 100644 wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/position_second_step1.c create mode 100644 wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/position_second_step2.c create mode 100644 wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/position_third_step1.c create mode 100644 wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/position_third_step2.c create mode 100644 wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/profile.c create mode 100644 wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/profile.h create mode 100644 wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/result.h create mode 100644 wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/roots.c create mode 100644 wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/roots.h create mode 100644 wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/trajectory.c create mode 100644 wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/trajectory.h create mode 100644 wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/utils.h create mode 100644 wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/velocity.h create mode 100644 wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/velocity_second_step1.c create mode 100644 wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/velocity_second_step2.c create mode 100644 wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/velocity_third_step1.c create mode 100644 wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/velocity_third_step2.c create mode 100644 wasm-port/vendor/linuxcnc/src/emc/tp/ruckig_wrapper.c create mode 100644 wasm-port/vendor/linuxcnc/src/emc/tp/ruckig_wrapper.h create mode 100644 wasm-port/vendor/linuxcnc/src/emc/tp/sp_scurve.c create mode 100644 wasm-port/vendor/linuxcnc/src/emc/tp/sp_scurve.h create mode 100644 wasm-port/vendor/linuxcnc/src/emc/tp/spherical_arc.c create mode 100644 wasm-port/vendor/linuxcnc/src/emc/tp/spherical_arc.h create mode 100644 wasm-port/vendor/linuxcnc/src/emc/tp/tc.c create mode 100644 wasm-port/vendor/linuxcnc/src/emc/tp/tc.h create mode 100644 wasm-port/vendor/linuxcnc/src/emc/tp/tc_types.h create mode 100644 wasm-port/vendor/linuxcnc/src/emc/tp/tcq.c create mode 100644 wasm-port/vendor/linuxcnc/src/emc/tp/tcq.h create mode 100644 wasm-port/vendor/linuxcnc/src/emc/tp/tp.c create mode 100644 wasm-port/vendor/linuxcnc/src/emc/tp/tp.h create mode 100644 wasm-port/vendor/linuxcnc/src/emc/tp/tp_debug.h create mode 100644 wasm-port/vendor/linuxcnc/src/emc/tp/tp_types.h create mode 100644 wasm-port/vendor/linuxcnc/src/libnml/posemath/_posemath.c create mode 100644 wasm-port/vendor/linuxcnc/src/libnml/posemath/gomath.c create mode 100644 wasm-port/vendor/linuxcnc/src/libnml/posemath/posemath.cc create mode 100644 wasm-port/vendor/linuxcnc/src/libnml/posemath/sincos.c create mode 100644 wasm-port/vendor/linuxcnc/src/rtapi/rtapi_atomic.h create mode 100644 wasm-port/vendor/linuxcnc/src/rtapi/rtapi_bool.h create mode 100644 wasm-port/vendor/linuxcnc/src/rtapi/rtapi_limits.h create mode 100644 wasm-port/vendor/linuxcnc/src/rtapi/rtapi_slab.h diff --git a/wasm-port/runtime/core/linuxcnc_wrap/linuxcnc_tp_api_probe.cpp b/wasm-port/runtime/core/linuxcnc_wrap/linuxcnc_tp_api_probe.cpp new file mode 100644 index 0000000..5ed9062 --- /dev/null +++ b/wasm-port/runtime/core/linuxcnc_wrap/linuxcnc_tp_api_probe.cpp @@ -0,0 +1,146 @@ +#include + +#include "emc/motion/motion.h" +#include "emc/nml_intf/motion_types.h" +#include "emc/tp/tp.h" + +namespace { + +emcmot_status_t status{}; +emcmot_config_t config{}; +emcmot_joint_t joints[EMCMOT_MAX_JOINTS]{}; + +void dio_write(int, char) {} +void aio_write(int, double) {} +void set_rotary_unlock(int, int) {} +int get_rotary_unlock(int) { return 1; } + +double axis_vel_limit(int axis) +{ + return axis >= 0 && axis < EMCMOT_MAX_AXIS ? 10.0 : 0.0; +} + +double axis_acc_limit(int axis) +{ + return axis >= 0 && axis < EMCMOT_MAX_AXIS ? 20.0 : 0.0; +} + +void init_motion_state() +{ + config.numJoints = 3; + config.numSpindles = 1; + config.numDIO = 4; + config.numAIO = 4; + config.arcBlendOptDepth = 0; + config.arcBlendEnable = 0; + config.arcBlendFallbackEnable = 0; + config.arcBlendGapCycles = 4; + config.arcBlendRampFreq = 20.0; + config.arcBlendTangentKinkRatio = 0.1; + config.maxFeedScale = 1.0; + + status.net_feed_scale = 1.0; + status.feed_scale = 1.0; + status.rapid_scale = 1.0; + status.enables_new = FS_ENABLED | SS_ENABLED; + status.enables_queued = status.enables_new; + status.spindle_status[0].at_speed = 1; + status.spindle_status[0].direction = 1; + status.spindleSync = 0; + status.jerk = 0.0; + status.planner_type = 0; + + tpMotData(&status, &config); + tpMotFunctions( + dio_write, + aio_write, + set_rotary_unlock, + get_rotary_unlock, + axis_vel_limit, + axis_acc_limit); +} + +void run_linear_probe() +{ + init_motion_state(); + + TP_STRUCT tp{}; + EmcPose start{}; + EmcPose end{}; + struct state_tag_t tag {}; + + end.tran.x = 1.0; + + const int create_rc = tpCreate(&tp, TP_DEFAULT_QUEUE_SIZE, 1); + const int set_cycle_rc = tpSetCycleTime(&tp, 0.001); + const int set_pos_rc = tpSetPos(&tp, &start); + const int set_vmax_rc = tpSetVmax(&tp, 1.0, 1.0); + const int set_vlimit_rc = tpSetVlimit(&tp, 1.0); + const int set_amax_rc = tpSetAmax(&tp, 10.0); + const int set_term_rc = tpSetTermCond(&tp, TC_TERM_COND_STOP, 0.0); + const int add_line_rc = tpAddLine( + &tp, + end, + EMC_MOTION_TYPE_FEED, + 1.0, + 1.0, + 10.0, + 100.0, + status.enables_new, + 0, + -1, + tag); + + int cycle_rc = 0; + int cycles = 0; + for (; cycles < 10000 && !tpIsDone(&tp); ++cycles) { + cycle_rc = tpRunCycle(&tp, 1000000); + if (cycle_rc < 0) { + break; + } + } + + EmcPose final_pos{}; + const int get_pos_rc = tpGetPos(&tp, &final_pos); + + std::cout << "tp_create=" << create_rc << "\n"; + std::cout << "tp_set_cycle_time=" << set_cycle_rc << "\n"; + std::cout << "tp_set_pos=" << set_pos_rc << "\n"; + std::cout << "tp_set_vmax=" << set_vmax_rc << "\n"; + std::cout << "tp_set_vlimit=" << set_vlimit_rc << "\n"; + std::cout << "tp_set_amax=" << set_amax_rc << "\n"; + std::cout << "tp_set_term_cond=" << set_term_rc << "\n"; + std::cout << "tp_add_line=" << add_line_rc << "\n"; + std::cout << "tp_cycle_rc=" << cycle_rc << "\n"; + std::cout << "tp_cycles=" << cycles << "\n"; + std::cout << "tp_get_pos=" << get_pos_rc << "\n"; + std::cout << "tp_done_after_line=" << tpIsDone(&tp) << "\n"; + std::cout << "tp_queue_depth=" << tpQueueDepth(&tp) << "\n"; + std::cout << "tp_final_pos=" + << final_pos.tran.x << "," + << final_pos.tran.y << "," + << final_pos.tran.z << "\n"; +} + +} // namespace + +int main() +{ + TP_STRUCT tp{}; + TC_STRUCT tc{}; + EmcPose pose{}; + + pose.tran.x = 1.0; + pose.tran.y = 2.0; + pose.tran.z = 3.0; + + std::cout << "sizeof_TP_STRUCT=" << sizeof(tp) << "\n"; + std::cout << "sizeof_TC_STRUCT=" << sizeof(tc) << "\n"; + std::cout << "tp_default_queue_size=" << TP_DEFAULT_QUEUE_SIZE << "\n"; + std::cout << "tp_err_ok=" << TP_ERR_OK << "\n"; + std::cout << "tc_linear=" << TC_LINEAR << "\n"; + std::cout << "tc_circular=" << TC_CIRCULAR << "\n"; + std::cout << "pose_xyz=" << pose.tran.x << "," << pose.tran.y << "," << pose.tran.z << "\n"; + run_linear_probe(); + return 0; +} diff --git a/wasm-port/runtime/core/shims/hal.h b/wasm-port/runtime/core/shims/hal.h index 0329cd8..a4ac8b2 100644 --- a/wasm-port/runtime/core/shims/hal.h +++ b/wasm-port/runtime/core/shims/hal.h @@ -23,6 +23,13 @@ typedef union { unsigned long long lu; } hal_data_u; +typedef bool hal_bit_t; +typedef double hal_float_t; +typedef int hal_s32_t; +typedef unsigned int hal_u32_t; +typedef long long hal_s64_t; +typedef unsigned long long hal_u64_t; + int hal_init(const char *); int hal_ready(int); int hal_get_pin_value_by_name(const char *, hal_type_t *, hal_data_u **, bool *); diff --git a/wasm-port/runtime/core/shims/rtapi.h b/wasm-port/runtime/core/shims/rtapi.h index 1f1af74..6feed6a 100644 --- a/wasm-port/runtime/core/shims/rtapi.h +++ b/wasm-port/runtime/core/shims/rtapi.h @@ -19,6 +19,10 @@ #define RTAPI_NAME_LEN 31 #endif +#ifndef EXPORT_SYMBOL +#define EXPORT_SYMBOL(symbol) +#endif + typedef enum { RTAPI_MSG_NONE = 0, RTAPI_MSG_ERR, diff --git a/wasm-port/tests/native/verify_native_probes.sh b/wasm-port/tests/native/verify_native_probes.sh index c3858db..b300a3c 100755 --- a/wasm-port/tests/native/verify_native_probes.sh +++ b/wasm-port/tests/native/verify_native_probes.sh @@ -32,6 +32,8 @@ check_exitcode() { } check_exitcode linuxcnc_interp_state_probe +check_exitcode linuxcnc_tp_api_probe +check_exitcode linuxcnc_tp_api_probe.run check_exitcode linuxcnc_namedparam_harness check_exitcode linuxcnc_namedparam_harness.run check_exitcode linuxcnc_interp_minimal_harness @@ -82,6 +84,18 @@ grep -Fq "saved_has_5399=1" "$PARAMETER_FILE_STDOUT" grep -Fq "saved_has_named_param=0" "$PARAMETER_FILE_STDOUT" grep -Fq "backup_has_original_5161=1" "$PARAMETER_FILE_STDOUT" +TP_API_STDOUT="$BUILD_DIR/linuxcnc_tp_api_probe.run.stdout.log" +grep -Fq "tp_default_queue_size=32" "$TP_API_STDOUT" +grep -Fq "tp_err_ok=0" "$TP_API_STDOUT" +grep -Fq "tc_linear=1" "$TP_API_STDOUT" +grep -Fq "tc_circular=2" "$TP_API_STDOUT" +grep -Fq "pose_xyz=1,2,3" "$TP_API_STDOUT" +grep -Fq "tp_create=0" "$TP_API_STDOUT" +grep -Fq "tp_set_cycle_time=0" "$TP_API_STDOUT" +grep -Fq "tp_add_line=0" "$TP_API_STDOUT" +grep -Fq "tp_done_after_line=1" "$TP_API_STDOUT" +grep -Fq "tp_final_pos=1,0,0" "$TP_API_STDOUT" + check_fixture_output() { local fixture="$1" local expected="$2" diff --git a/wasm-port/tools/build_native_probes.sh b/wasm-port/tools/build_native_probes.sh index 746fb45..402a0ac 100755 --- a/wasm-port/tools/build_native_probes.sh +++ b/wasm-port/tools/build_native_probes.sh @@ -27,6 +27,8 @@ COMMON_FLAGS=( -I"$VENDOR_DIR/src/emc" -I"$VENDOR_DIR/src/emc/nml_intf" -I"$VENDOR_DIR/src/emc/motion" + -I"$VENDOR_DIR/src/emc/tp" + -I"$VENDOR_DIR/src/emc/kinematics" -I"$VENDOR_DIR/src/libnml/posemath" ) @@ -47,6 +49,11 @@ MINIMAL_LINK_FLAGS=( -Wl,--gc-sections ) +TP_FLAGS=( + "${COMMON_FLAGS[@]}" + -fpermissive +) + write_command_file() { local file="$1" shift @@ -251,6 +258,48 @@ STATE_PROBE_SOURCES=( "$WRAP_DIR/linuxcnc_interp_state_probe.cpp" ) +TP_API_PROBE_SOURCES=( + "$WRAP_DIR/linuxcnc_tp_api_probe.cpp" +) + +TP_CORE_SOURCES=( + "$VENDOR_DIR/src/emc/tp/tp.c" + "$VENDOR_DIR/src/emc/tp/tc.c" + "$VENDOR_DIR/src/emc/tp/tcq.c" + "$VENDOR_DIR/src/emc/tp/spherical_arc.c" + "$VENDOR_DIR/src/emc/tp/blendmath.c" + "$VENDOR_DIR/src/emc/tp/sp_scurve.c" + "$VENDOR_DIR/src/emc/tp/ruckig_wrapper.c" + "$VENDOR_DIR/src/emc/tp/cruckig/block.c" + "$VENDOR_DIR/src/emc/tp/cruckig/brake.c" + "$VENDOR_DIR/src/emc/tp/cruckig/calculator.c" + "$VENDOR_DIR/src/emc/tp/cruckig/cruckig.c" + "$VENDOR_DIR/src/emc/tp/cruckig/input_parameter.c" + "$VENDOR_DIR/src/emc/tp/cruckig/output_parameter.c" + "$VENDOR_DIR/src/emc/tp/cruckig/profile.c" + "$VENDOR_DIR/src/emc/tp/cruckig/roots.c" + "$VENDOR_DIR/src/emc/tp/cruckig/trajectory.c" + "$VENDOR_DIR/src/emc/tp/cruckig/position_first_step1.c" + "$VENDOR_DIR/src/emc/tp/cruckig/position_first_step2.c" + "$VENDOR_DIR/src/emc/tp/cruckig/position_second_step1.c" + "$VENDOR_DIR/src/emc/tp/cruckig/position_second_step2.c" + "$VENDOR_DIR/src/emc/tp/cruckig/position_third_step1.c" + "$VENDOR_DIR/src/emc/tp/cruckig/position_third_step2.c" + "$VENDOR_DIR/src/emc/tp/cruckig/velocity_second_step1.c" + "$VENDOR_DIR/src/emc/tp/cruckig/velocity_second_step2.c" + "$VENDOR_DIR/src/emc/tp/cruckig/velocity_third_step1.c" + "$VENDOR_DIR/src/emc/tp/cruckig/velocity_third_step2.c" + "$VENDOR_DIR/src/emc/nml_intf/emcpose.c" + "$VENDOR_DIR/src/libnml/posemath/posemath.cc" + "$VENDOR_DIR/src/libnml/posemath/_posemath.c" + "$VENDOR_DIR/src/libnml/posemath/sincos.c" +) + +TP_RUNTIME_PROBE_SOURCES=( + "${TP_CORE_SOURCES[@]}" + "$WRAP_DIR/linuxcnc_tp_api_probe.cpp" +) + INTERP_CORE_SOURCES=( "$VENDOR_DIR/src/emc/rs274ngc/modal_state.cc" "$VENDOR_DIR/src/emc/rs274ngc/interp_array.cc" @@ -307,6 +356,28 @@ build_binary_target \ STATE_PROBE_SOURCES \ NO_LINK_FLAGS +build_binary_target \ + linuxcnc_tp_api_probe \ + "$BUILD_DIR/linuxcnc_tp_api_probe" \ + TP_FLAGS \ + TP_RUNTIME_PROBE_SOURCES \ + NO_LINK_FLAGS + +if [[ "$(tr -d '[:space:]' < "$BUILD_DIR/linuxcnc_tp_api_probe.exitcode")" == "0" ]]; then + set +e + "$BUILD_DIR/linuxcnc_tp_api_probe" \ + >"$BUILD_DIR/linuxcnc_tp_api_probe.run.stdout.log" \ + 2>"$BUILD_DIR/linuxcnc_tp_api_probe.run.stderr.log" + TP_API_RUN_RC=$? + set -e + echo "$TP_API_RUN_RC" > "$BUILD_DIR/linuxcnc_tp_api_probe.run.exitcode" +else + rm -f \ + "$BUILD_DIR/linuxcnc_tp_api_probe.run.exitcode" \ + "$BUILD_DIR/linuxcnc_tp_api_probe.run.stdout.log" \ + "$BUILD_DIR/linuxcnc_tp_api_probe.run.stderr.log" +fi + build_binary_target \ linuxcnc_namedparam_harness \ "$BUILD_DIR/linuxcnc_namedparam_harness" \ diff --git a/wasm-port/tools/source-manifest.txt b/wasm-port/tools/source-manifest.txt index 38a7e48..8444b6c 100644 --- a/wasm-port/tools/source-manifest.txt +++ b/wasm-port/tools/source-manifest.txt @@ -2,12 +2,18 @@ src/emc/ini/inifile.cc src/emc/ini/inifile.h src/emc/ini/inifile.hh src/rtapi/rtapi_stdint.h +src/rtapi/rtapi_bool.h +src/rtapi/rtapi_limits.h +src/rtapi/rtapi_atomic.h +src/rtapi/rtapi_slab.h src/rtapi/rtapi_string.h src/rtapi/rtapi_gfp.h src/rtapi/rtapi_math.h src/rtapi/rtapi_byteorder.h src/emc/nml_intf/emcpos.h src/emc/nml_intf/emcpose.h +src/emc/nml_intf/emcpose.c +src/emc/nml_intf/motion_types.h src/emc/linuxcnc.h src/emc/nml_intf/canon.hh src/emc/nml_intf/canon_position.hh @@ -17,11 +23,71 @@ src/emc/nml_intf/debugflags.h src/emc/nml_intf/interp_return.hh src/emc/motion/state_tag.h src/emc/motion/emcmotcfg.h +src/emc/motion/simple_tp.h +src/emc/motion/motion.h +src/emc/motion/mot_priv.h +src/emc/motion/axis.h +src/emc/kinematics/kinematics.h +src/emc/kinematics/cubic.h +src/emc/tp/tp.h +src/emc/tp/tp_types.h +src/emc/tp/tc.h +src/emc/tp/tc_types.h +src/emc/tp/tcq.h +src/emc/tp/spherical_arc.h +src/emc/tp/blendmath.h +src/emc/tp/sp_scurve.h +src/emc/tp/ruckig_wrapper.h +src/emc/tp/tp_debug.h +src/emc/tp/tp.c +src/emc/tp/tc.c +src/emc/tp/tcq.c +src/emc/tp/spherical_arc.c +src/emc/tp/blendmath.c +src/emc/tp/sp_scurve.c +src/emc/tp/ruckig_wrapper.c +src/emc/tp/cruckig/block.h +src/emc/tp/cruckig/brake.h +src/emc/tp/cruckig/calculator.h +src/emc/tp/cruckig/cruckig.h +src/emc/tp/cruckig/cruckig_internal.h +src/emc/tp/cruckig/input_parameter.h +src/emc/tp/cruckig/output_parameter.h +src/emc/tp/cruckig/position.h +src/emc/tp/cruckig/profile.h +src/emc/tp/cruckig/result.h +src/emc/tp/cruckig/roots.h +src/emc/tp/cruckig/trajectory.h +src/emc/tp/cruckig/utils.h +src/emc/tp/cruckig/velocity.h +src/emc/tp/cruckig/block.c +src/emc/tp/cruckig/brake.c +src/emc/tp/cruckig/calculator.c +src/emc/tp/cruckig/cruckig.c +src/emc/tp/cruckig/input_parameter.c +src/emc/tp/cruckig/output_parameter.c +src/emc/tp/cruckig/profile.c +src/emc/tp/cruckig/roots.c +src/emc/tp/cruckig/trajectory.c +src/emc/tp/cruckig/position_first_step1.c +src/emc/tp/cruckig/position_first_step2.c +src/emc/tp/cruckig/position_second_step1.c +src/emc/tp/cruckig/position_second_step2.c +src/emc/tp/cruckig/position_third_step1.c +src/emc/tp/cruckig/position_third_step2.c +src/emc/tp/cruckig/velocity_second_step1.c +src/emc/tp/cruckig/velocity_second_step2.c +src/emc/tp/cruckig/velocity_third_step1.c +src/emc/tp/cruckig/velocity_third_step2.c src/emc/rs274ngc/modal_state.hh src/emc/rs274ngc/modal_state.cc src/libnml/posemath/posemath.h +src/libnml/posemath/posemath.cc +src/libnml/posemath/_posemath.c +src/libnml/posemath/gomath.c src/libnml/posemath/gomath.h src/libnml/posemath/gotypes.h +src/libnml/posemath/sincos.c src/libnml/posemath/sincos.h src/emc/rs274ngc/interp_parameter_def.hh src/emc/rs274ngc/interp_array.cc diff --git a/wasm-port/vendor/linuxcnc/src/emc/kinematics/cubic.h b/wasm-port/vendor/linuxcnc/src/emc/kinematics/cubic.h new file mode 100644 index 0000000..8c066a4 --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/kinematics/cubic.h @@ -0,0 +1,62 @@ +/******************************************************************** +* Description: cubic.h +* Cubic polynomial interpolation code +* +* Derived from a work by Fred Proctor & Will Shackleford +* +* Author: +* License: GPL Version 2 +* System: Linux +* +* Copyright (c) 2004 All rights reserved. +********************************************************************/ +#ifndef CUBIC_H +#define CUBIC_H + +/* + Coefficients of a cubic polynomial, + + a * x^3 + b * x^2 + c * x + d +*/ + +typedef struct { + double a; + double b; + double c; + double d; +} CUBIC_COEFF; + +typedef struct { + int configured; + double segmentTime; + int interpolationRate; + double interpolationTime; + double interpolationIncrement; + double x0, x1, x2, x3; + double wp0, wp1; + double velp0, velp1; + int filled; + int needNextPoint; + CUBIC_COEFF coeff; +} CUBIC_STRUCT; + +extern int cubicInit(CUBIC_STRUCT * ci); +extern int cubicSetSegmentTime(CUBIC_STRUCT * ci, double time); +extern double cubicGetSegmentTime(CUBIC_STRUCT * ci); +extern int cubicSetInterpolationRate(CUBIC_STRUCT * ci, int rate); +extern int cubicGetInterpolationRate(CUBIC_STRUCT * ci); +extern int cubicAddPoint(CUBIC_STRUCT * ci, double point); +extern int cubicOffset(CUBIC_STRUCT * ci, double offset); +extern double cubicGetInterpolationIncrement(CUBIC_STRUCT * ci); +extern CUBIC_COEFF cubicGetCubicCoeff(CUBIC_STRUCT * ci); +extern int cubicFilled(CUBIC_STRUCT * ci); +extern double cubicInterpolate(CUBIC_STRUCT * ci, double *x, /* same as + return val + */ + double *v, /* velocity */ + double *a, /* accel */ + double *j); /* jerk */ +extern int cubicNeedNextPoint(CUBIC_STRUCT * ci); +extern int cubicDrain(CUBIC_STRUCT * ci); + +#endif /* CUBIC_H */ diff --git a/wasm-port/vendor/linuxcnc/src/emc/kinematics/kinematics.h b/wasm-port/vendor/linuxcnc/src/emc/kinematics/kinematics.h new file mode 100644 index 0000000..67e6565 --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/kinematics/kinematics.h @@ -0,0 +1,215 @@ +/******************************************************************** +* Description: kinematics.h +* +* Derived from a work by Fred Proctor & Will Shackleford +* +* Author: +* License: GPL Version 2 +* System: Linux +* +* Copyright (c) 2004 All rights reserved. +* +* Last change: +********************************************************************/ + +#ifndef __LINUXCNC_KINEMATICS_H +#define __LINUXCNC_KINEMATICS_H + +#include "emcpos.h" /* EmcPose */ +#include "rtapi_bool.h" + +/* + The type of kinematics used. + + KINEMATICS_IDENTITY means that the joints and world coordinates are the + same, as for slideway machines (XYZ milling machines). The EMC will allow + changing from joint to world mode and vice versa. Also, the EMC will set + the actual world position to be the actual joint positions (not commanded) + by calling the forward kinematics each trajectory cycle. + + KINEMATICS_FORWARD_ONLY means that only the forward kinematics exist. + Since the EMC requires at least the inverse kinematics, this should simply + terminate the EMC. + + KINEMATICS_INVERSE_ONLY means that only the inverse kinematics exist. + The forwards won't be called, and the EMC will only allow changing from + joint to world mode at the home position. + + KINEMATICS_BOTH means that both the forward and inverse kins are defined. + Like KINEMATICS_IDENTITY, the EMC will allow changing between world and + joint modes. However, the kins are assumed to be somewhat expensive + computationally, and the forwards won't be called at the trajectory rate + to compute actual world coordinates from actual joint values. +*/ + +typedef enum { + KINEMATICS_IDENTITY = 1,/* forward=inverse, both well-behaved */ + KINEMATICS_FORWARD_ONLY,/* forward but no inverse */ + KINEMATICS_INVERSE_ONLY,/* inverse but no forward */ + KINEMATICS_BOTH /* forward and inverse both */ +} KINEMATICS_TYPE; + +/* the forward flags are passed to the forward kinematics so that they + can resolve ambiguities in the world coordinates for a given joint set, + e.g., for hexpods, this would be platform-below-base, platform-above-base. + + The flags are also passed to the inverse kinematics and are set by them, + which is how they are changed from their initial value. For example, for + hexapods you could do a coordinated move that brings the platform up from + below the base to above the base. The forward flags would be set to + indicate this. */ +typedef unsigned long int KINEMATICS_FORWARD_FLAGS; + +/* the inverse flags are passed to the inverse kinematics so that they + can resolve ambiguities in the joint angles for a given world coordinate, + e.g., for robots, this would be elbow-up, elbow-down, etc. + + The flags are also passed to the forward kinematics and are set by them, + which is how they are changed from their initial value. For example, for + robots you could do a joint move that brings the elbow from a down + configuration to an up configuration. The inverse flags would be set to + indicate this. */ +typedef unsigned long int KINEMATICS_INVERSE_FLAGS; + +/* the forward kinematics take joint values and determine world coordinates, + given forward kinematics flags to resolve any ambiguities. The inverse + flags are set to indicate their value appropriate to the joint values + passed in. */ +extern int kinematicsForward(const double *joint, + struct EmcPose * world, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags); + +/* the inverse kinematics take world coordinates and determine joint values, + given the inverse kinematics flags to resolve any ambiguities. The forward + flags are set to indicate their value appropriate to the world coordinates + passed in. */ +extern int kinematicsInverse(const struct EmcPose * world, + double *joint, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags); + +/* the home kinematics function sets all its arguments to their proper + values at the known home position. When called, these should be set, + when known, to initial values, e.g., from an INI file. If the home + kinematics can accept arbitrary starting points, these initial values + should be used. +*/ +extern int kinematicsHome(struct EmcPose * world, + double *joint, + KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags); + +extern KINEMATICS_TYPE kinematicsType(void); + +/* parameters for use with switchkins.c */ +typedef struct kinematics_parms { + char* sparm; // module string parameter passed to kins + char* kinsname; // must agree with module(file) name + char* halprefix; // for hal pin hames + char* required_coordinates; + int max_joints; + int allow_duplicates; + int fwd_iterates_mask; // identify kins types that use iterative + // forward kinematics (typ: genhex) + // bitmask: 0x0 none + // bitmask: 0x1 bit0: switchkins_type==0 + // bitmask: 0x2 bit1: switchkins_type==1 + // bitmask: 0x4 bit2: switchkins_type==2 + int gui_kinstype; // may be reqd for parallel kins with vismach + // to select switchkins_type for gui pins +} kparms; + +/* map letters in a coordinates string to joint numbers +** sequentially. Axis indices are 0:x,1:y,...,etc +** Example: coordinates=XYZYAC +** Result: axis_idx_for_jno[0] = 0 ==> X +** axis_idx_for_jno[1] = 1 ==> Y +** axis_idx_for_jno[2] = 2 ==> Z +** axis_idx_for_jno[3] = 1 ==> Y (duplicate allowed) +** axis_idx_for_jno[4] = 1 ==> A +** axis_idx_for_jno[5] = 1 ==> C +*/ +extern int map_coordinates_to_jnumbers(const char *coordinates, + const int max_joints, + const int allow_duplicates, + int axis_idx_for_jno[]); + +extern int mapped_joints_to_position(const int max_joints, + const double* joints, + EmcPose* pose); + +extern int position_to_mapped_joints(const int max_joints, + const EmcPose* pos, + double* joints); + +extern int identityKinematicsSetup(const int comp_id, + const char* coordinates, + kparms* ksetup_parms); + +extern int identityKinematicsForward(const double *joint, + struct EmcPose * world, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags); + +extern int identityKinematicsInverse(const struct EmcPose * world, + double *joint, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags); + +extern int kinematicsSwitchable(void); +extern int kinematicsSwitch(int switchkins_type); +//NOTE: switchable kinematics may require Interp::Synch +// before/after invoking kinematicsSwitch() +// A convenient command to synch is: M66 E0 L0 + +#define KINS_NOT_SWITCHABLE \ +extern int kinematicsSwitchable() {return 0;} \ +extern int kinematicsSwitch(int switchkins_type) { (void)switchkins_type; return 0;} \ +EXPORT_SYMBOL(kinematicsSwitchable); \ +EXPORT_SYMBOL(kinematicsSwitch); + + +// support for template for user-defined switchkins_type==2 +extern int userkKinematicsSetup(const int comp_id, + const char* coordinates, + kparms* ksetup_parms); + +extern int userkKinematicsForward(const double *joint, + struct EmcPose * world, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags); + +extern int userkKinematicsInverse(const struct EmcPose * world, + double *joint, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags); +#endif +//********************************************************************* +// xyzac,xyzbc; +extern int trtKinematicsSetup(const int comp_id, + const char* coordinates, + kparms* ksetup_parms); + +extern int xyzacKinematicsForward(const double *joints, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags); + +extern int xyzacKinematicsInverse(const EmcPose * pos, + double *joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags); + + +extern int xyzbcKinematicsForward(const double *joints, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags); + +extern int xyzbcKinematicsInverse(const EmcPose * pos, + double *joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags); + +//********************************************************************* diff --git a/wasm-port/vendor/linuxcnc/src/emc/motion/axis.h b/wasm-port/vendor/linuxcnc/src/emc/motion/axis.h new file mode 100644 index 0000000..5d61f0b --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/motion/axis.h @@ -0,0 +1,60 @@ + +#ifndef AXIS_H +#define AXIS_H + +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +void axis_init_all(void); +void axis_initialize_external_offsets(void); +int axis_init_hal_io(int mot_comp_id); + +void axis_handle_jogwheels(bool motion_teleop_flag, bool motion_enable_flag, bool homing_is_active); +bool axis_plan_external_offsets(double servo_period, bool motion_enable_flag, bool all_homed); +void axis_check_constraints(double pos[], int failing_axes[]); + +void axis_jog_cont(int axis_num, double vel, long servo_period); +void axis_jog_incr(int axis_num, double offset, double vel, long servo_period); +void axis_jog_abs(int axis_num, double offset, double vel); +bool axis_jog_abort_all(bool immediate); +bool axis_jog_abort(int axis_num, bool immediate); +bool axis_jog_is_active(void); + +void axis_output_to_hal(double *pcmd_p[]); + +void axis_set_max_pos_limit(int axis_num, double maxLimit); +void axis_set_min_pos_limit(int axis_num, double minLimit); +void axis_set_vel_limit(int axis_num, double vel); +void axis_set_acc_limit(int axis_num, double acc); +void axis_set_jerk_limit(int axis_num, double jerk); +void axis_set_ext_offset_vel_limit(int axis_num, double ext_offset_vel); +void axis_set_ext_offset_acc_limit(int axis_num, double ext_offset_acc); +void axis_set_locking_joint(int axis_num, int joint); + +double axis_get_min_pos_limit(int axis_num); +double axis_get_max_pos_limit(int axis_num); +double axis_get_vel_limit(int axis_num); +double axis_get_acc_limit(int axis_num); +int axis_get_locking_joint(int axis_num); +double axis_get_compound_velocity(void); +double axis_get_ext_offset_curr_pos(int axis_num); + +double axis_get_teleop_vel_cmd(int axis_num); + +void axis_sync_teleop_tp_to_carte_pos(int extfactor, double *pcmd_p[]); +void axis_sync_carte_pos_to_teleop_tp(int extfactor, double *pcmd_p[]); +void axis_apply_ext_offsets_to_carte_pos(int extfactor, double *pcmd_p[]); + +int axis_update_coord_with_bound(double *pcmd_p[], double servo_period); + +int axis_calc_motion(double servo_period); + + +#ifdef __cplusplus +} +#endif +#endif /* AXIS_H */ diff --git a/wasm-port/vendor/linuxcnc/src/emc/motion/mot_priv.h b/wasm-port/vendor/linuxcnc/src/emc/motion/mot_priv.h new file mode 100644 index 0000000..ff3a2a0 --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/motion/mot_priv.h @@ -0,0 +1,355 @@ +/******************************************************************* +* Description: mot_priv.h +* Macros and declarations local to the realtime sources. +* +* Author: +* License: GPL Version 2 +* System: Linux +* +* Copyright (c) 2004 All rights reserved. +********************************************************************/ +#ifndef MOT_PRIV_H +#define MOT_PRIV_H + +/*********************************************************************** +* TYPEDEFS, ENUMS, ETC. * +************************************************************************/ + +/* First we define structures for data shared with the HAL */ + +/* HAL visible data notations: + RPA: read only parameter + WPA: write only parameter + WRPA: read/write parameter + RPI: read only pin + WPI: write only pin + WRPI: read/write pin +*/ + +/* joint data */ +#include +#include "../motion/motion.h" + +typedef struct { + // creating a lot of pins for spindle control to be very flexible + // the user needs only a subset of these + + // simplest way of spindle control (output start/stop) + hal_bit_t *spindle_on; /* spindle spin output */ + + // same thing for 2 directions + hal_bit_t *spindle_forward; /* spindle spin-forward output */ + hal_bit_t *spindle_reverse; /* spindle spin-reverse output */ + + // simple velocity control (as long as the output is active the spindle + // should accelerate/decelerate + hal_bit_t *spindle_incr_speed; /* spindle spin-increase output */ + hal_bit_t *spindle_decr_speed; /* spindle spin-decrease output */ + + // simple output for brake + hal_bit_t *spindle_brake; /* spindle brake output */ + + // output of a prescribed speed (to hook-up to a velocity controller) + hal_float_t *spindle_speed_out; /* spindle speed output */ + hal_float_t *spindle_speed_out_rps; /* spindle speed output */ + hal_float_t *spindle_speed_out_abs; /* spindle speed output absolute*/ + hal_float_t *spindle_speed_out_rps_abs; /* spindle speed output absolute*/ + hal_float_t *spindle_speed_cmd_rps; /* spindle speed command without SO applied */ + hal_float_t *spindle_speed_in; /* spindle speed measured */ + hal_bit_t *spindle_index_enable; /* spindle inde I/O pin */ + hal_bit_t *spindle_inhibit; + hal_float_t *spindle_revs; + hal_bit_t *spindle_is_atspeed; + hal_bit_t *spindle_amp_fault; + + // spindle orient + hal_float_t *spindle_orient_angle; /* out: desired spindle angle, degrees */ + hal_s32_t *spindle_orient_mode; /* out: 0: least travel; 1: cw; 2: ccw */ + hal_bit_t *spindle_orient; /* out: signal orient in progress */ + hal_bit_t *spindle_locked; /* out: signal orient complete, spindle locked */ + hal_bit_t *spindle_is_oriented; /* in: orientation completed */ + hal_s32_t *spindle_orient_fault; /* in: error code of failed operation */ + +} spindle_hal_t; + +typedef struct { + hal_float_t *coarse_pos_cmd;/* RPI: commanded position, w/o comp */ + hal_float_t *joint_vel_cmd; /* RPI: commanded velocity, w/o comp */ + hal_float_t *joint_acc_cmd; /* RPI: commanded acceleration, w/o comp */ + hal_float_t *joint_jerk_cmd;/* RPI: commanded jerk, w/o comp */ + hal_float_t *backlash_corr; /* RPI: correction for backlash */ + hal_float_t *backlash_filt; /* RPI: filtered backlash correction */ + hal_float_t *backlash_vel; /* RPI: backlash speed variable */ + hal_float_t *motor_offset; /* RPI: motor offset, for checking homing stability */ + hal_float_t *motor_pos_cmd; /* WPI: commanded position, with comp */ + hal_float_t *motor_pos_fb; /* RPI: position feedback, with comp */ + hal_float_t *joint_pos_cmd; /* WPI: commanded position w/o comp, not ofs */ + hal_float_t *joint_pos_fb; /* RPI: position feedback, w/o comp */ + hal_float_t *f_error; /* RPI: following error */ + hal_float_t *f_error_lim; /* RPI: following error limit */ + + hal_float_t *free_pos_cmd; /* RPI: free traj planner pos cmd */ + hal_float_t *free_vel_lim; /* RPI: free traj planner vel limit */ + hal_bit_t *free_tp_enable; /* RPI: free traj planner is running */ + hal_bit_t *kb_jjog_active; /* RPI: executing keyboard jog */ + hal_bit_t *wheel_jjog_active;/* RPI: executing handwheel jog */ + + hal_bit_t *active; /* RPI: joint is active, whatever that means */ + hal_bit_t *in_position; /* RPI: joint is in position */ + hal_bit_t *error; /* RPI: joint has an error */ + hal_bit_t *phl; /* RPI: joint is at positive hard limit */ + hal_bit_t *nhl; /* RPI: joint is at negative hard limit */ + hal_bit_t *f_errored; /* RPI: joint had too much following error */ + hal_bit_t *faulted; /* RPI: joint amp faulted */ + hal_bit_t *pos_lim_sw; /* RPI: positive limit switch input */ + hal_bit_t *neg_lim_sw; /* RPI: negative limit switch input */ + hal_bit_t *amp_fault; /* RPI: amp fault input */ + hal_bit_t *amp_enable; /* WPI: amp enable output */ + + hal_bit_t *unlock; /* WPI: command that axis should unlock for rotation */ + hal_bit_t *is_unlocked; /* RPI: axis is currently unlocked */ + + hal_s32_t *jjog_counts; /* WPI: jogwheel position input */ + hal_bit_t *jjog_enable; /* RPI: enable jogwheel */ + hal_float_t *jjog_scale; /* RPI: distance to jog on each count */ + hal_float_t *jjog_accel_fraction; /* RPI: to limit wheel jog accel */ + hal_bit_t *jjog_vel_mode; /* RPI: true for "velocity mode" jogwheel */ +} joint_hal_t; + +typedef struct { + hal_float_t *posthome_cmd; // IN pin extrajoint +} extrajoint_hal_t; + +/* machine data */ + +typedef struct { + hal_bit_t *probe_input; /* RPI: probe switch input */ + hal_bit_t *enable; /* RPI: motion inhibit input */ + hal_float_t *adaptive_feed; /* RPI: adaptive feedrate, 0.0 to 1.0 */ + hal_bit_t *feed_hold; /* RPI: set TRUE to stop motion maskable with g53 P1*/ + hal_bit_t *feed_inhibit; /* RPI: set TRUE to stop motion (non maskable)*/ + hal_bit_t *homing_inhibit; /* RPI: set TRUE to inhibit homing*/ + hal_bit_t *jog_inhibit; /* RPI: set TRUE to inhibit jogging*/ + hal_bit_t *jog_stop; /* RPI: set TRUE to stop jogging following accel values*/ + hal_bit_t *jog_stop_immediate; /* RPI: set TRUE to stop jogging immediately*/ + hal_bit_t *jog_is_active; /* RPI: TRUE if active jogging*/ + hal_bit_t *tp_reverse; /* Set true if trajectory planner is running in reverse*/ + hal_bit_t *motion_enabled; /* RPI: motion enable for all joints */ + hal_bit_t *is_all_homed; /* RPI: TRUE if all active joints is homed */ + hal_bit_t *in_position; /* RPI: all joints are in position */ + hal_bit_t *coord_mode; /* RPA: TRUE if coord, FALSE if free */ + hal_bit_t *teleop_mode; /* RPA: TRUE if teleop mode */ + hal_bit_t *coord_error; /* RPA: TRUE if coord mode error */ + hal_bit_t *on_soft_limit; /* RPA: TRUE if outside a limit */ + + hal_s32_t *program_line; /* RPA: program line causing current motion */ + hal_s32_t *motion_type; /* RPA: type (feed/rapid) of currently commanded motion */ + hal_float_t *current_vel; /* RPI: velocity magnitude in machine units */ + hal_float_t *requested_vel; /* RPI: requested velocity magnitude in machine units */ + hal_float_t *distance_to_go;/* RPI: distance to go in current move*/ + + hal_bit_t debug_bit_0; /* RPA: generic param, for debugging */ + hal_bit_t debug_bit_1; /* RPA: generic param, for debugging */ + hal_float_t debug_float_0; /* RPA: generic param, for debugging */ + hal_float_t debug_float_1; /* RPA: generic param, for debugging */ + hal_float_t debug_float_2; /* RPA: generic param, for debugging */ + hal_float_t debug_float_3; /* RPA: generic param, for debugging */ + hal_s32_t debug_s32_0; /* RPA: generic param, for debugging */ + hal_s32_t debug_s32_1; /* RPA: generic param, for debugging */ + + hal_bit_t *synch_do[EMCMOT_MAX_DIO]; /* WPI array: output pins for motion synched IO */ + hal_bit_t *synch_di[EMCMOT_MAX_DIO]; /* RPI array: input pins for motion synched IO */ + hal_float_t *analog_input[EMCMOT_MAX_AIO]; /* RPI array: input pins for analog Inputs */ + hal_float_t *analog_output[EMCMOT_MAX_AIO]; /* RPI array: output pins for analog Inputs */ + hal_bit_t *misc_error[EMCMOT_MAX_MISC_ERROR]; /* RPI array: output pins for misc error Inputs */ + + // FIXME - debug only, remove later + hal_float_t traj_pos_out; /* RPA: traj internals, for debugging */ + hal_float_t traj_vel_out; /* RPA: traj internals, for debugging */ + hal_u32_t traj_active_tc; /* RPA: traj internals, for debugging */ + hal_float_t tc_pos[4]; /* RPA: traj internals, for debugging */ + hal_float_t tc_vel[4]; /* RPA: traj internals, for debugging */ + hal_float_t tc_acc[4]; /* RPA: traj internals, for debugging */ + + // realtime overrun detection + hal_u32_t *last_period; /* pin: last period in clocks */ + hal_float_t *last_period_ns; /* pin: last period in nanoseconds */ + + hal_float_t *tooloffset_x; + hal_float_t *tooloffset_y; + hal_float_t *tooloffset_z; + hal_float_t *tooloffset_a; + hal_float_t *tooloffset_b; + hal_float_t *tooloffset_c; + hal_float_t *tooloffset_u; + hal_float_t *tooloffset_v; + hal_float_t *tooloffset_w; + + spindle_hal_t spindle[EMCMOT_MAX_SPINDLES]; /*spindle data */ + joint_hal_t joint[EMCMOT_MAX_JOINTS]; /* data for each joint */ + extrajoint_hal_t ejoint[EMCMOT_MAX_EXTRAJOINTS]; /* data for each extrajoint */ + + hal_bit_t *eoffset_active; /* ext offsets active */ + hal_bit_t *eoffset_limited; /* ext offsets exceed limit */ + + hal_float_t *feed_upm; /* feed G-code units per minute*/ + hal_float_t *feed_inches_per_minute; /* feed inches per minute*/ + hal_float_t *feed_inches_per_second; /* feed inches per second*/ + hal_float_t *feed_mm_per_minute; /* feed mm per minute*/ + hal_float_t *feed_mm_per_second; /* feed mm per second*/ + + hal_float_t *switchkins_type; + /* Interp State Pins */ + hal_s32_t *interp_line_number; + hal_s32_t *interp_motion_type; + hal_float_t *interp_feedrate; + + /* New Geometric Metadata Pins */ + hal_float_t *interp_arc_radius; + hal_float_t *interp_arc_center_x; + hal_float_t *interp_arc_center_y; + hal_float_t *interp_arc_center_z; + hal_float_t *interp_straight_heading; + hal_float_t *interp_normal_heading; + hal_bit_t *iscircle; +} emcmot_hal_data_t; + +/*********************************************************************** +* GLOBAL VARIABLE DECLARATIONS * +************************************************************************/ + +/* pointer to emcmot_hal_data_t struct in HAL shmem, with all HAL data */ +extern emcmot_hal_data_t *emcmot_hal_data; + +/* pointer to array of joint structs with all joint data */ +/* the actual array may be in shared memory or in kernel space, as + determined by the init code in motion.c */ +extern emcmot_joint_t joints[EMCMOT_MAX_JOINTS]; + +/* Variable defs */ +extern KINEMATICS_FORWARD_FLAGS fflags; +extern KINEMATICS_INVERSE_FLAGS iflags; +/* these variable have the 1/servo cycle time */ + +/* Struct pointers */ +extern struct emcmot_struct_t *emcmotStruct; +extern struct emcmot_command_t *emcmotCommand; +extern struct emcmot_status_t *emcmotStatus; +extern struct emcmot_config_t *emcmotConfig; +extern struct emcmot_internal_t *emcmotInternal; +extern struct emcmot_error_t *emcmotError; + +/*********************************************************************** +* PUBLIC FUNCTION PROTOTYPES * +************************************************************************/ + +/* function definitions */ +extern void emcmotCommandHandler(void *arg, long period); +extern void emcmotController(void *arg, long period); +extern void emcmotSetCycleTime(unsigned long nsec); + +/* these are related to synchronized I/O */ +extern void emcmotDioWrite(int index, char value); +extern void emcmotAioWrite(int index, double value); + +extern void emcmotSetRotaryUnlock(int axis, int unlock); +extern int emcmotGetRotaryIsUnlocked(int axis); + +// +// Try to change the Motion mode to Teleop. +// +// This function can be called at any time. Returns without changing +// mode if Teleop is not currently allowed. This code doesn't actually +// make the transition, it just sets a flag requesting the transition. +// The real transition to Teleop mode is done in emcmotController(). +// +void switch_to_teleop_mode(void); + +/* recalculates jog limits */ +extern void refresh_jog_limits(emcmot_joint_t *joint,int joint_num); +/* handles 'homed' flags, see command.c for details */ +extern void clearHomes(int joint_num); + +extern void emcmot_config_change(void); +extern void reportError(const char *fmt, ...) __attribute__((format(printf,1,2))); /* Use the rtapi_print call */ + + +int joint_is_lockable(int joint_num); + +#define ALL_JOINTS emcmotConfig->numJoints +// number of kinematics-only joints: +#define NO_OF_KINS_JOINTS (ALL_JOINTS - emcmotConfig->numExtraJoints) +#define IS_EXTRA_JOINT(jno) (jno >= NO_OF_KINS_JOINTS) +// 0-based Joint numbering: +// kinematic-only jno.s: [0 ... (NO_OF_KINS_JOINTS -1) ] +// extrajoint jno.s: [NO_OF_KINS_JOINTS ... (ALL_JOINTS -1) ] + + /* rtapi_get_time() returns a nanosecond value. In time, we should use a u64 + value for all calcs and only do the conversion to seconds when it is + really needed. */ +#define etime() (((double) rtapi_get_time()) / 1.0e9) + +/* macros for reading, writing bit flags */ + +/* motion flags */ + +#define GET_MOTION_ERROR_FLAG() (emcmotStatus->motionFlag & EMCMOT_MOTION_ERROR_BIT ? 1 : 0) + +#define SET_MOTION_ERROR_FLAG(fl) if (fl) emcmotStatus->motionFlag |= EMCMOT_MOTION_ERROR_BIT; else emcmotStatus->motionFlag &= ~EMCMOT_MOTION_ERROR_BIT; + +#define GET_MOTION_COORD_FLAG() (emcmotStatus->motionFlag & EMCMOT_MOTION_COORD_BIT ? 1 : 0) + +#define SET_MOTION_COORD_FLAG(fl) if (fl) emcmotStatus->motionFlag |= EMCMOT_MOTION_COORD_BIT; else emcmotStatus->motionFlag &= ~EMCMOT_MOTION_COORD_BIT; + +#define GET_MOTION_TELEOP_FLAG() (emcmotStatus->motionFlag & EMCMOT_MOTION_TELEOP_BIT ? 1 : 0) + +#define SET_MOTION_TELEOP_FLAG(fl) if (fl) emcmotStatus->motionFlag |= EMCMOT_MOTION_TELEOP_BIT; else emcmotStatus->motionFlag &= ~EMCMOT_MOTION_TELEOP_BIT; + +#define GET_MOTION_INPOS_FLAG() (emcmotStatus->motionFlag & EMCMOT_MOTION_INPOS_BIT ? 1 : 0) + +#define SET_MOTION_INPOS_FLAG(fl) if (fl) emcmotStatus->motionFlag |= EMCMOT_MOTION_INPOS_BIT; else emcmotStatus->motionFlag &= ~EMCMOT_MOTION_INPOS_BIT; + +#define GET_MOTION_ENABLE_FLAG() (emcmotStatus->motionFlag & EMCMOT_MOTION_ENABLE_BIT ? 1 : 0) + +#define SET_MOTION_ENABLE_FLAG(fl) if (fl) emcmotStatus->motionFlag |= EMCMOT_MOTION_ENABLE_BIT; else emcmotStatus->motionFlag &= ~EMCMOT_MOTION_ENABLE_BIT; + +#define GET_TRAJ_PLANNER_TYPE() (emcmotStatus->planner_type) + +#define SET_TRAK_PLANNER_TYPE(tp) (emcmotStatus->planner_type = tp) + +/* joint flags */ + +#define GET_JOINT_ENABLE_FLAG(joint) ((joint)->flag & EMCMOT_JOINT_ENABLE_BIT ? 1 : 0) + +#define SET_JOINT_ENABLE_FLAG(joint,fl) if (fl) (joint)->flag |= EMCMOT_JOINT_ENABLE_BIT; else (joint)->flag &= ~EMCMOT_JOINT_ENABLE_BIT; + +#define SET_JOINT_ACTIVE_FLAG(joint,fl) if (fl) (joint)->flag |= EMCMOT_JOINT_ACTIVE_BIT; else (joint)->flag &= ~EMCMOT_JOINT_ACTIVE_BIT; + +#define SET_JOINT_INPOS_FLAG(joint,fl) if (fl) (joint)->flag |= EMCMOT_JOINT_INPOS_BIT; else (joint)->flag &= ~EMCMOT_JOINT_INPOS_BIT; + +#define GET_JOINT_ERROR_FLAG(joint) ((joint)->flag & EMCMOT_JOINT_ERROR_BIT ? 1 : 0) + +#define SET_JOINT_ERROR_FLAG(joint,fl) if (fl) (joint)->flag |= EMCMOT_JOINT_ERROR_BIT; else (joint)->flag &= ~EMCMOT_JOINT_ERROR_BIT; + +#define GET_JOINT_PHL_FLAG(joint) ((joint)->flag & EMCMOT_JOINT_MAX_HARD_LIMIT_BIT ? 1 : 0) + +#define SET_JOINT_PHL_FLAG(joint,fl) if (fl) (joint)->flag |= EMCMOT_JOINT_MAX_HARD_LIMIT_BIT; else (joint)->flag &= ~EMCMOT_JOINT_MAX_HARD_LIMIT_BIT; + +#define GET_JOINT_NHL_FLAG(joint) ((joint)->flag & EMCMOT_JOINT_MIN_HARD_LIMIT_BIT ? 1 : 0) + +#define SET_JOINT_NHL_FLAG(joint,fl) if (fl) (joint)->flag |= EMCMOT_JOINT_MIN_HARD_LIMIT_BIT; else (joint)->flag &= ~EMCMOT_JOINT_MIN_HARD_LIMIT_BIT; + + +#define GET_JOINT_FERROR_FLAG(joint) ((joint)->flag & EMCMOT_JOINT_FERROR_BIT ? 1 : 0) + +#define SET_JOINT_FERROR_FLAG(joint,fl) if (fl) (joint)->flag |= EMCMOT_JOINT_FERROR_BIT; else (joint)->flag &= ~EMCMOT_JOINT_FERROR_BIT; + +#define GET_JOINT_FAULT_FLAG(joint) ((joint)->flag & EMCMOT_JOINT_FAULT_BIT ? 1 : 0) + +#define SET_JOINT_FAULT_FLAG(joint,fl) if (fl) (joint)->flag |= EMCMOT_JOINT_FAULT_BIT; else (joint)->flag &= ~EMCMOT_JOINT_FAULT_BIT; + +#if defined(__KERNEL__) +#define HAVE_CPU_KHZ +#endif + +#endif /* MOT_PRIV_H */ diff --git a/wasm-port/vendor/linuxcnc/src/emc/motion/motion.h b/wasm-port/vendor/linuxcnc/src/emc/motion/motion.h new file mode 100644 index 0000000..9c62af0 --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/motion/motion.h @@ -0,0 +1,780 @@ +/******************************************************************** +* Description: motion.h +* Data structures used throughout emc2. +* +* Author: +* License: GPL Version 2 +* System: Linux +* +* Copyright (c) 2004 All rights reserved +********************************************************************/ + +/* jmk says: This file is a mess! */ + +/* + +Misc ramblings: + +The terms axis and joint are used inconsistently throughout EMC. +For all new code, the usages are as follows: + + axis - one of the nine degrees of freedom, x, y, z, a, b, c, u, v, w + these refer to axes in Cartesian space, which may or + may not match up with joints (see below). On Cartesian + machines they do match up, but for hexapods, robots, and + other non-Cartesian machines they don't. + joint - one of the physical degrees of freedom of the machine + these might be linear (leadscrews) or rotary (rotary + tables, robot arm joints). There can be any number of + joints. The kinematics code is responsible for translating + from axis space to joint space and back. + +There are three main kinds of data needed by the motion controller + +1) data shared with higher level stuff - commands, status, etc. +2) data that is local to the motion controller +3) data shared with lower level stuff - hal pins + +In addition, some internal data (2) should be shared for trouble +shooting purposes, even though it is "internal" to the motion +controller. Depending on the type of data, it can either be +treated as type (1), and made available to the higher level +code, or it can be treated as type (3), and made available to +the hal, so that halscope can monitor it. + +This file should ONLY contain structures and declarations for +type (1) items - those that are shared with higher level code. + +Type (2) items should be declared in mot_priv.h, along +with type (3) items. + +In the interest of retaining my sanity, I'm not gonna attempt +to move everything to its proper location yet.... + +However, all new items will be defined in the proper place, +and some existing items may be moved from one struct definition +to another. + +*/ + +#ifndef MOTION_H +#define MOTION_H + +#include +#include + +#include +#include +#include /* PmCartesian, PmPose, pmCartMag() */ +#include /* EmcPose */ +#include "../kinematics/cubic.h" /* CUBIC_STRUCT, CUBIC_COEFF */ +#include /* EMCMOT_MAX_JOINTS */ +#include + +#include "simple_tp.h" +#include "state_tag.h" +#include "../tp/tp_types.h" + +// define a special value to denote an invalid motion ID +// NB: do not ever generate a motion id of MOTION_INVALID_ID +// this should be really be tested for in command.c + +#define MOTION_INVALID_ID INT_MIN +#define MOTION_ID_VALID(x) ((x) != MOTION_INVALID_ID) + +#include /* must precede rtapi_atomic.h in kernel mode */ +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/* This enum lists all the possible commands */ + + typedef enum { + EMCMOT_ABORT = 1, /* abort all motion */ + EMCMOT_ENABLE, /* enable servos for active joints */ + EMCMOT_DISABLE, /* disable servos for active joints */ + + EMCMOT_PAUSE, /* pause motion */ + EMCMOT_REVERSE, /* run reverse motion */ + EMCMOT_FORWARD, /* run reverse motion */ + EMCMOT_RESUME, /* resume motion */ + EMCMOT_STEP, /* resume motion until id encountered */ + EMCMOT_FREE, /* set mode to free (joint) motion */ + EMCMOT_COORD, /* set mode to coordinated motion */ + EMCMOT_TELEOP, /* set mode to teleop */ + + EMCMOT_SPINDLE_SCALE, /* set scale factor for spindle speed */ + EMCMOT_SS_ENABLE, /* enable/disable scaling the spindle speed */ + EMCMOT_FEED_SCALE, /* set scale factor for feedrate */ + EMCMOT_RAPID_SCALE, /* set scale factor for rapids */ + EMCMOT_FS_ENABLE, /* enable/disable scaling feedrate */ + EMCMOT_FH_ENABLE, /* enable/disable feed_hold */ + EMCMOT_AF_ENABLE, /* enable/disable adaptive feedrate */ + EMCMOT_OVERRIDE_LIMITS, /* temporarily ignore limits until jog done */ + + EMCMOT_SET_LINE, /* queue up a linear move */ + EMCMOT_SET_CIRCLE, /* queue up a circular move */ + EMCMOT_SET_TELEOP_VECTOR, /* Move at a given velocity but in + world cartesian coordinates, not + in joint space like EMCMOT_JOG_* */ + EMCMOT_CLEAR_PROBE_FLAGS, /* clears probeTripped flag */ + EMCMOT_PROBE, /* go to pos, stop if probe trips, record + trip pos */ + EMCMOT_RIGID_TAP, /* go to pos, with sync to spindle speed, + then return to initial pos */ + + EMCMOT_SET_VEL, /* set the velocity for subsequent moves */ + EMCMOT_SET_VEL_LIMIT, /* set the max vel for all moves (tooltip) */ + EMCMOT_SET_ACC, /* set the max accel for moves (tooltip) */ + EMCMOT_SET_JERK, /* set the max jerk for moves (tooltip) */ + EMCMOT_SET_PLANNER_TYPE, /* set planner type (0=trapezoidal, 1=S-curve) */ + EMCMOT_SET_TERM_COND, /* set termination condition (stop, blend) */ + EMCMOT_SET_NUM_JOINTS, /* set the number of joints */ + EMCMOT_SET_NUM_SPINDLES, /* set the number of spindles */ + EMCMOT_SET_WORLD_HOME, /* set pose for world home */ + + EMCMOT_SET_DEBUG, /* sets the debug level */ + EMCMOT_SET_DOUT, /* sets or unsets a DIO, this can be immediate or synched with motion */ + EMCMOT_SET_AOUT, /* sets or unsets a AIO, this can be immediate or synched with motion */ + EMCMOT_SET_SPINDLESYNC, /* synchronize motion to spindle encoder */ + EMCMOT_SPINDLE_ON, /* start the spindle */ + EMCMOT_SPINDLE_OFF, /* stop the spindle */ + EMCMOT_SPINDLE_INCREASE, /* spindle faster */ + EMCMOT_SPINDLE_DECREASE, /* spindle slower */ + EMCMOT_SPINDLE_BRAKE_ENGAGE, /* engage the spindle brake */ + EMCMOT_SPINDLE_BRAKE_RELEASE, /* release the spindle brake */ + EMCMOT_SPINDLE_ORIENT, /* orient the spindle */ + EMCMOT_SET_OFFSET, /* set tool offsets */ + EMCMOT_SET_MAX_FEED_OVERRIDE, + EMCMOT_SETUP_ARC_BLENDS, + + EMCMOT_SET_PROBE_ERR_INHIBIT, + EMCMOT_ENABLE_WATCHDOG, /* enable watchdog sound, parport */ + EMCMOT_DISABLE_WATCHDOG, /* enable watchdog sound, parport */ + EMCMOT_JOG_CONT, /* continuous jog */ + EMCMOT_JOG_INCR, /* incremental jog */ + EMCMOT_JOG_ABS, /* absolute jog */ + + EMCMOT_JOG_ABORT, /* abort one joint num or axis num */ + EMCMOT_JOINT_ACTIVATE, /* make joint active */ + EMCMOT_JOINT_DEACTIVATE, /* make joint inactive */ + EMCMOT_JOINT_HOME, /* home a joint or all joints */ + EMCMOT_JOINT_UNHOME, /* unhome a joint or all joints*/ + EMCMOT_SET_JOINT_POSITION_LIMITS, /* set the joint position +/- limits */ + EMCMOT_SET_JOINT_BACKLASH, /* set the joint backlash */ + EMCMOT_SET_JOINT_MIN_FERROR, /* minimum following error, input units */ + EMCMOT_SET_JOINT_MAX_FERROR, /* maximum following error, input units */ + EMCMOT_SET_JOINT_VEL_LIMIT, /* set the max joint vel */ + EMCMOT_SET_JOINT_ACC_LIMIT, /* set the max joint accel */ + EMCMOT_SET_JOINT_HOMING_PARAMS, /* sets joint homing parameters */ + EMCMOT_SET_JOINT_JERK_LIMIT, /* set the max joint jerk */ + EMCMOT_UPDATE_JOINT_HOMING_PARAMS, /* updates some joint homing parameters */ + EMCMOT_SET_JOINT_MOTOR_OFFSET, /* set the offset between joint and motor */ + EMCMOT_SET_JOINT_COMP, /* set a compensation triplet for a joint (nominal, forw., rev.) */ + + EMCMOT_SET_AXIS_POSITION_LIMITS, /* set the axis position +/- limits */ + EMCMOT_SET_AXIS_VEL_LIMIT, /* set the max axis vel */ + EMCMOT_SET_AXIS_ACC_LIMIT, /* set the max axis acc */ + EMCMOT_SET_AXIS_LOCKING_JOINT, /* set the axis locking joint */ + EMCMOT_SET_AXIS_JERK_LIMIT, /* set the max axis jerk */ + + EMCMOT_SET_SPINDLE_PARAMS, /* One command to set all spindle params */ + + } cmd_code_t; + +/* this enum lists the possible results of a command */ + + typedef enum { + EMCMOT_COMMAND_OK = 0, /* cmd honored */ + EMCMOT_COMMAND_UNKNOWN_COMMAND, /* cmd not understood */ + EMCMOT_COMMAND_INVALID_COMMAND, /* cmd can't be handled now */ + EMCMOT_COMMAND_INVALID_PARAMS, /* bad cmd params */ + EMCMOT_COMMAND_BAD_EXEC /* error trying to initiate */ + } cmd_status_t; + +/* termination conditions for queued motions */ +#define EMCMOT_TERM_COND_STOP 1 +#define EMCMOT_TERM_COND_BLEND 2 +#define EMCMOT_TERM_COND_TANGENT 3 + +/********************************* + COMMAND STRUCTURE +*********************************/ + +/* This is the command structure. There is one of these in shared + memory, and all commands from higher level code come thru it. +*/ + typedef struct emcmot_command_t { + cmd_code_t command; /* command code (enum) */ + int commandNum; /* increment this for new command */ + double motor_offset; /* offset from joint to motor position */ + double maxLimit; /* pos value for position limit, output */ + double minLimit; /* neg value for position limit, output */ + double min_pos_speed; /* spindle minimum positive speed */ + double max_neg_speed; /* spindle maximum negative speed */ + EmcPose pos; /* line/circle endpt, or teleop vector */ + PmCartesian center; /* center for circle */ + PmCartesian normal; /* normal vec for circle */ + int turn; /* turns for circle or joint number for a locking indexer*/ + double vel; /* max velocity */ + double ini_maxvel; /* max velocity allowed by machine + constraints (the INI file) */ + int motion_type; /* this move is because of traverse, feed, arc, or toolchange */ + double spindlesync; /* user units per spindle revolution, 0 = no sync */ + double acc; /* max acceleration */ + double jerk; /* jerk for traj */ + double ini_maxjerk; + int planner_type; /* planner type: 0 = trapezoidal, 1 = S-curve */ + double backlash; /* amount of backlash */ + int id; /* id for motion */ + int termCond; /* termination condition */ + double tolerance; /* tolerance for path deviation in CONTINUOUS mode */ + int joint; /* which joint index to use for below */ + int axis; /* which axis index to use for below */ + int spindle; /* which spindle to use */ + double scale; /* velocity scale or spindle_speed scale arg */ + double offset; /* input, output, or home offset arg */ + double home; /* joint home position */ + double home_final_vel; /* joint velocity for moving from OFFSET to HOME */ + double search_vel; /* home search velocity */ + double latch_vel; /* home latch velocity */ + int flags; /* homing config flags, other boolean args */ + int home_sequence; /* order in homing sequence */ + int volatile_home; /* joint should get unhomed when we get unhome -2 + (generated by task upon estop, etc) */ + double minFerror; /* min following error */ + double maxFerror; /* max following error */ + int wdWait; /* cycle to wait before toggling wd */ + int debug; /* debug level, from DEBUG in INI file */ + unsigned char now, out, start, end; /* these are related to synched AOUT/DOUT. now=whether now or synched, out = which gets set, start=start value, end=end value */ + unsigned char mode; /* used for turning overrides etc. on/off */ + double comp_nominal, comp_forward, comp_reverse; /* compensation triplet, nominal, forward, reverse */ + unsigned char probe_type; /* ~1 = error if probe operation is unsuccessful (ngc default) + |1 = suppress error, report in # instead + ~2 = move until probe trips (ngc default) + |2 = move until probe clears */ + int probe_jog_err_inhibit; // setting to inhibit probe tripped while jogging error. + int probe_home_err_inhibit; // setting to inhibit probe tripped while homeing error. + EmcPose tool_offset; /* TLO */ + double orientation; /* angle for spindle orient */ + int state; /*spindle state seems to just be 0 for off and 1 for on andypugh 2025-04-03*/ + char direction; /* CANON_DIRECTION flag for spindle orient */ + double timeout; /* of wait for spindle orient to complete */ + unsigned char wait_for_spindle_at_speed; // EMCMOT_SPINDLE_ON now carries this, for next feed move + int arcBlendOptDepth; + int arcBlendEnable; + int arcBlendFallbackEnable; + int arcBlendGapCycles; + double arcBlendRampFreq; + double arcBlendTangentKinkRatio; + double maxFeedScale; + double ext_offset_vel; /* velocity for an external axis offset */ + double ext_offset_acc; /* acceleration for an external axis offset */ + struct state_tag_t tag; + } emcmot_command_t; + +/*! \todo FIXME - these packed bits might be replaced with chars + memory is cheap, and being able to access them without those + damn macros would be nice +*/ + +/* motion flag type */ + typedef unsigned short EMCMOT_MOTION_FLAG; + +/* + motion status flag structure-- looks like: + + MSB LSB + v---------------v------------------v + | | | | T | CE | C | IP | EN | + ^---------------^------------------^ + + where: + + EN is 1 if calculations are enabled, 0 if not + IP is 1 if all joints in position, 0 if not + C is 1 if coordinated mode, 0 if in free mode + CE is 1 if coordinated mode error, 0 if not + T is 1 if we are in teleop mode. + */ + +/* bit masks */ +#define EMCMOT_MOTION_ENABLE_BIT 0x0001 +#define EMCMOT_MOTION_INPOS_BIT 0x0002 +#define EMCMOT_MOTION_COORD_BIT 0x0004 +#define EMCMOT_MOTION_ERROR_BIT 0x0008 +#define EMCMOT_MOTION_TELEOP_BIT 0x0010 + +/* joint flag type */ + typedef unsigned short EMCMOT_JOINT_FLAG; +/* + joint status flag structure-- looks like: + + MSB LSB + ----------v-----------------v--------------------v-------------------v + | AF | FE | AH | HD | H | HS | NHL | PHL | - | - | ER | IP | AC | EN | + ----------^-----------------^--------------------^-------------------^ + + + x = unused + + where: + + EN is 1 if joint amplifier is enabled, 0 if not + AC is 1 if joint is active for calculations, 0 if not + IP is 1 if joint is in position, 0 if not (free mode only) + ER is 1 if joint has an error, 0 if not + + PHL is 1 if joint is on maximum hardware limit, 0 if not + NHL is 1 if joint is on minimum hardware limit, 0 if not + + HS is 1 if joint home switch is tripped, 0 if not + H is 1 if joint is homing, 0 if not + HD is 1 if joint has been homed, 0 if not + AH is 1 if joint is at home position, 0 if not + + FE is 1 if joint exceeded following error, 0 if not + AF is 1 if amplifier is faulted, 0 if not + +Suggestion: Split this in to an Error and a Status flag register.. + Then a simple test on each of the two flags can be performed + rather than testing each bit... Saving on a global per joint + fault and ready status flag. + */ + +/* bit masks */ +#define EMCMOT_JOINT_ENABLE_BIT 0x0001 +#define EMCMOT_JOINT_ACTIVE_BIT 0x0002 +#define EMCMOT_JOINT_INPOS_BIT 0x0004 +#define EMCMOT_JOINT_ERROR_BIT 0x0008 +#define EMCMOT_JOINT_MAX_HARD_LIMIT_BIT 0x0010 +#define EMCMOT_JOINT_MIN_HARD_LIMIT_BIT 0x0020 +#define EMCMOT_JOINT_FERROR_BIT 0x0040 +#define EMCMOT_JOINT_FAULT_BIT 0x0080 + +/*! \todo FIXME - the terms "teleop", "coord", and "free" are poorly + documented. This is my feeble attempt to understand exactly + what they mean. + + According to Fred, teleop is never used with machine tools, + although that may not be true for machines with non-trivial + kinematics. + + "coord", or coordinated mode, means that all the joints are + synchronized, and move together as commanded by the higher + level code. It is the normal mode when machining. In + coordinated mode, commands are assumed to be in the cartesean + reference frame, and if the machine is non-cartesean, the + commands are translated by the kinematics to drive each + joint in joint space as needed. + + "free" mode means commands are interpreted in joint space. + It is used for jogging individual joints, although + it does not preclude multiple joints moving at once (I think). + Homing is also done in free mode, in fact machines with + non-trivial kinematics must be homed before they can go + into either coord or teleop mode. + + 'teleop' is what you probably want if you are 'jogging' + a hexapod. The jog commands as implemented by the motion + controller are joint jogs, which work in free mode. But + if you want to jog a hexapod or similar machine along + one particular cartesean axis, you need to operate more + than one joint. That's what 'teleop' is for. + +*/ + +/* compensation structures */ + typedef struct { + double nominal; /* nominal (command) position */ + float fwd_trim; /* correction for forward movement */ + float rev_trim; /* correction for reverse movement */ + float fwd_slope; /* slopes between here and next pt */ + float rev_slope; + } emcmot_comp_entry_t; + + +#define EMCMOT_COMP_SIZE 256 + typedef struct { + int entries; /* number of entries in the array */ + emcmot_comp_entry_t *entry; /* current entry in array */ + emcmot_comp_entry_t array[EMCMOT_COMP_SIZE+2]; + /* +2 because array has -HUGE_VAL and +HUGE_VAL entries at the ends */ + } emcmot_comp_t; + +/* motion controller states */ + + typedef enum { + EMCMOT_MOTION_DISABLED = 0, + EMCMOT_MOTION_FREE, + EMCMOT_MOTION_TELEOP, + EMCMOT_MOTION_COORD + } motion_state_t; + + + typedef enum { + EMCMOT_ORIENT_NONE = 0, + EMCMOT_ORIENT_COMPLETE, + EMCMOT_ORIENT_IN_PROGRESS, + EMCMOT_ORIENT_FAULTED, + } orient_state_t; + +/* flags for enabling spindle scaling, feed scaling, + adaptive feed, and feed hold */ + +#define SS_ENABLED 0x01 +#define FS_ENABLED 0x02 +#define AF_ENABLED 0x04 +#define FH_ENABLED 0x08 + +/* This structure contains all of the data associated with + a single joint. Note that this structure does not need + to be in shared memory (but it can, if desired for debugging + reasons). The portions of this structure that are considered + "status" and need to be made available to user space are + copied to a much smaller struct called emcmot_joint_status_t + which is located in shared memory. + +*/ + typedef struct { + + /* configuration info - changes rarely */ + int type; /* 0 = linear, 1 = rotary */ + double max_pos_limit; /* upper soft limit on joint pos */ + double min_pos_limit; /* lower soft limit on joint pos */ + double max_jog_limit; /* jog limits change when not homed */ + double min_jog_limit; + double vel_limit; /* upper limit of joint speed */ + double acc_limit; /* upper limit of joint accel */ + double jerk_limit; /* upper limit of joint jerk */ + double min_ferror; /* zero speed following error limit */ + double max_ferror; /* max speed following error limit */ + double backlash; /* amount of backlash */ + emcmot_comp_t comp; /* leadscrew correction data */ + + /* status info - changes regularly */ + /* many of these need to be made available to higher levels */ + /* they can either be copied to the status struct, or an array of + joint structs can be made part of the status */ + EMCMOT_JOINT_FLAG flag; /* see above for bit details */ + double coarse_pos; /* trajectory point, before interp */ + double pos_cmd; /* commanded joint position */ + double vel_cmd; /* commanded joint velocity */ + double acc_cmd; /* commanded joint acceleration */ + double jerk_cmd; /* comanded joint jerk */ + double backlash_corr; /* correction for backlash */ + double backlash_filt; /* filtered backlash correction */ + double backlash_vel; /* backlash velocity variable */ + double motor_pos_cmd; /* commanded position, with comp */ + double motor_pos_fb; /* position feedback, with comp */ + double pos_fb; /* position feedback, comp removed */ + double ferror; /* following error */ + double ferror_limit; /* limit depends on speed */ + double ferror_high_mark; /* max following error */ + simple_tp_t free_tp; /* planner for free mode motion */ + int kb_jjog_active; /* non-zero during a keyboard jog */ + int wheel_jjog_active; /* non-zero during a wheel jog */ + + /* internal info - changes regularly, not usually accessed from user + space */ + CUBIC_STRUCT cubic; /* cubic interpolator data */ + + int on_pos_limit; /* non-zero if on limit */ + int on_neg_limit; /* non-zero if on limit */ + + double motor_offset; /* diff between internal and motor pos, used + to set position to zero during homing */ + int old_jjog_counts; /* prior value, used for deltas */ + double big_vel; /* used for "debouncing" velocity */ + } emcmot_joint_t; + +/* This structure contains only the "status" data associated with + a joint. "Status" data is that data that should be reported to + user space on a continuous basis. An array of these structs is + part of the main status structure, and is filled in with data + copied from the emcmot_joint_t structs every servo period. + + For now this struct contains more data than it really needs, but + paring it down will take time (and probably needs to be done one + or two items at a time, with much testing). My main goal right + now is to get get the large joint struct out of status. + +*/ + typedef struct { + EMCMOT_JOINT_FLAG flag; /* see above for bit details */ + bool homed; + bool homing; + + double pos_cmd; /* commanded joint position */ + double pos_fb; /* position feedback, comp removed */ + double vel_cmd; /* current velocity */ + double acc_cmd; /* current acceleration */ + double ferror; /* following error */ + double ferror_high_mark; /* max following error */ + +/*! \todo FIXME - the following are not really "status", but taskintf.cc expects + them to be in the status structure. I don't know how or if they are + used by the user space code. Ideally they will be removed from here, + but each one will need to be investigated individually. +*/ + double backlash; /* amount of backlash */ + double max_pos_limit; /* upper soft limit on joint pos */ + double min_pos_limit; /* lower soft limit on joint pos */ + double min_ferror; /* zero speed following error limit */ + double max_ferror; /* max speed following error limit */ + } emcmot_joint_status_t; + + + typedef struct { + double speed; // spindle speed in RPMs + double scale; // spindle override value + double net_scale; // scale or zero if inhibited + double css_factor; + double xoffset; + int state; + int direction; // 0 stopped, 1 forward, -1 reverse + int brake; // 0 released, 1 engaged + int locked; // spindle lock engaged after orient + int orient_fault; // fault code from motion.spindle-orient-fault + int orient_state; // orient_state_t + int spindle_index_enable; /* hooked to a canon encoder index-enable */ + double spindleRevs; /* position of spindle in revolutions */ + double spindleSpeedIn; /* velocity of spindle in revolutions per minute */ + int at_speed; + int fault; /* amplifier fault */ + double max_pos_speed; /* spindle speed limits */ + double min_pos_speed; /* signed values, so max_neg = 0 */ + double max_neg_speed; /* and min_neg = -1e99 indicates no limit */ + double min_neg_speed; + double home_angle; + double home_search_vel; + int home_sequence; + double increment; + } spindle_status_t; + + typedef struct { + double teleop_vel_cmd; /* commanded axis velocity */ + double max_pos_limit; /* upper soft limit on axis pos */ + double min_pos_limit; /* lower soft limit on axis pos */ + } emcmot_axis_status_t; + +/********************************* + STATUS STRUCTURE +*********************************/ + +/* This is the status structure. There is one of these in shared + memory, and it reports motion controller status to higher level + code in user space. For the most part, this structure contains + higher level variables - low level stuff is made visible to the + HAL and troubleshooting, etc, is done using the HAL oscilloscope. +*/ + +/*! \todo FIXME - this struct is broken into two parts... at the top are + structure members that I understand, and that are needed for emc2. + Other structure members follow. All the later ones need to be + evaluated - either they move up, or they go away. +*/ + + typedef struct emcmot_status_t { + unsigned char head; /* flag count for mutex detect */ + /* these three are updated only when a new command is handled */ + cmd_code_t commandEcho; /* echo of input command */ + int commandNumEcho; /* echo of input command number */ + cmd_status_t commandStatus; /* result of most recent command */ + /* these are config info, updated when a command changes them */ + double feed_scale; /* velocity scale factor for all motion but rapids */ + double rapid_scale; /* velocity scale factor for rapids */ + unsigned char enables_new; /* flags for FS, SS, etc */ + /* the above set is the enables in effect for new moves */ + /* the rest are updated every cycle */ + double net_feed_scale; /* net scale factor for all motion */ + unsigned char enables_queued; /* flags for FS, SS, etc */ + /* the above set is the enables in effect for the + currently executing move */ + motion_state_t motion_state; /* operating state: FREE, COORD, etc. */ + EMCMOT_MOTION_FLAG motionFlag; /* see above for bit details */ + EmcPose carte_pos_cmd; /* commanded Cartesian position */ + int carte_pos_cmd_ok; /* non-zero if command is valid */ + EmcPose carte_pos_fb; /* actual Cartesian position */ + int carte_pos_fb_ok; /* non-zero if feedback is valid */ + EmcPose world_home; /* cartesean coords of home position */ + emcmot_joint_status_t joint_status[EMCMOT_MAX_JOINTS]; /* all joint status data */ + emcmot_axis_status_t axis_status[EMCMOT_MAX_AXIS]; /* all axis status data */ + int spindleSync; /* spindle used for synchronised moves. -1 = none */ + spindle_status_t spindle_status[EMCMOT_MAX_SPINDLES]; /* all spindle data */ + + + int on_soft_limit; /* non-zero if any joint is on soft limit */ + + int probeVal; /* debounced value of probe input */ + + int probeTripped; /* Has the probe signal changed since start + of probe command? */ + int probing; /* Currently looking for a probe signal? */ + unsigned char probe_type; + EmcPose probedPos; /* Axis positions stored as soon as possible + after last probeTripped */ + + + int synch_di[EMCMOT_MAX_DIO]; /* inputs to the motion controller, queried by G-code */ + int synch_do[EMCMOT_MAX_DIO]; /* outputs to the motion controller, queried by G-code */ + double analog_input[EMCMOT_MAX_AIO]; /* inputs to the motion controller, queried by G-code */ + double analog_output[EMCMOT_MAX_AIO]; /* outputs to the motion controller, queried by G-code */ + int misc_error[EMCMOT_MAX_MISC_ERROR]; /* Random Error pins*/ + struct state_tag_t tag; /* Current interp state corresponding + to motion line */ + +/*! \todo FIXME - all structure members beyond this point are in limbo */ + + /* dynamic status-- changes every cycle */ + uint64_t heartbeat; /* Incremented every time the motion controller is done. */ + int config_num; /* incremented whenever configuration + changed. */ + int id; /* id for executing motion */ + int depth; /* motion queue depth */ + int activeDepth; /* depth of active blend elements */ + int queueFull; /* Flag to indicate the tc queue is full */ + int paused; /* Flag to signal motion paused */ + int overrideLimitMask; /* non-zero means one or more limits ignored */ + /* 1 << (joint-num*2) = ignore neg limit */ + /* 2 << (joint-num*2) = ignore pos limit */ + int reverse_run; + + /* static status-- only changes upon input commands, e.g., config */ + double vel; /* scalar max vel */ + double acc; /* scalar max accel */ + double jerk; /* jerk for traj */ + int planner_type; /* planner type: 0 = trapezoidal, 1 = S-curve */ + + int motionType; + double distance_to_go; /* in this move */ + EmcPose dtg; + double current_vel; + double requested_vel; + + /* S-curve motion state - for accurate jerk output */ + double current_acc; /* current path acceleration */ + double current_jerk; /* current path jerk (accurate value from TP) */ + double decel_dist; /* S-curve deceleration distance (dlen1) for debugging */ + PmCartesian current_dir; /* current motion direction unit vector */ + + unsigned int tcqlen; + EmcPose tool_offset; + int atspeed_next_feed; /* at next feed move, wait for spindle to be at speed */ + unsigned char tail; /* flag count for mutex detect */ + int external_offsets_applied; + EmcPose eoffset_pose; + int numExtraJoints; + int stepping; + bool jogging_active; + } emcmot_status_t; + +/********************************* + CONFIG STRUCTURE +*********************************/ + +/* This is the config structure. This is currently in shared memory, + but I have no idea why... there are commands to set most of the + items in this structure. It seems we should either put the struct + in private memory and manipulate it with commands, or we should + put it in shared memory and manipulate it directly - not both. + The structure contains static or rarely changed information that + describes the machine configuration. + + later: I think I get it now - the struct is in shared memory so + user space can read the config at any time, but commands are used + to change the config so they only take effect when the realtime + code processes the command. +*/ + +/*! \todo FIXME - this struct is broken into two parts... at the top are + structure members that I understand, and that are needed for emc2. + Other structure members follow. All the later ones need to be + evaluated - either they move up, or they go away. +*/ + typedef struct emcmot_config_t { + unsigned char head; /* flag count for mutex detect */ + + int config_num; /* Incremented everytime configuration + changed, should match status.config_num */ + int numJoints; /* The number of total joints in the system (which + must be between 1 and EMCMOT_MAX_JOINTS, + inclusive). includes extra joints*/ + int numExtraJoints; /* The number of extra joints in the system (which + must be between 1 and EMCMOT_MAX_EXTRAJOINTS, + inclusive). */ + int numSpindles; /* The number of spindles, 1 to EMCMOT_MAX_SPINDLES */ + + KINEMATICS_TYPE kinType; + + int numDIO; /* userdefined number of digital IO. default is 4. (EMCMOT_MAX_DIO=64), + but can be altered at motmod insmod time */ + + int numAIO; /* userdefined number of analog IO. default is 4. (EMCMOT_MAX_AIO=16), + but can be altered at motmod insmod time */ + + int numMiscError; /* userdefined number of Misc Errors. default is 0. + but can be altered at motmod insmod time */ + +/*! \todo FIXME - all structure members beyond this point are in limbo */ + + double trajCycleTime; /* the rate at which the trajectory loop + runs.... (maybe) */ + double servoCycleTime; /* the rate of the servo loop - Not the same + as the traj time */ + + int interpolationRate; /* grep control.c for an explanation.... + approx line 50 */ + + double limitVel; /* scalar upper limit on vel */ + int debug; /* copy of DEBUG, from INI file */ + unsigned char tail; /* flag count for mutex detect */ + int arcBlendOptDepth; + int arcBlendEnable; + int arcBlendFallbackEnable; + int arcBlendGapCycles; + double arcBlendRampFreq; + double arcBlendTangentKinkRatio; + double maxFeedScale; + int inhibit_probe_jog_error; + int inhibit_probe_home_error; + } emcmot_config_t; + +/* error structure - lockfree MPSC ring buffer. See emcmotutil.c. */ + typedef struct emcmot_error_t { + char error[EMCMOT_ERROR_NUM][EMCMOT_ERROR_LEN]; + rtapi_atomic_ullong write_reserve; + rtapi_atomic_ullong write_commit; + rtapi_atomic_ullong read_seq; + } emcmot_error_t; + + +typedef struct emcmot_internal_t { + unsigned char head; /* flag count for mutex detect */ + unsigned char tail; /* flag count for mutex detect */ + int split; /* number of split command reads */ + int enabling; /* starts up disabled */ + int coordinating; /* starts up in free mode */ + int teleoperating; /* starts up in free mode */ + int overriding; /* non-zero means we've initiated an joint + move while overriding limits */ + TP_STRUCT coord_tp; /* coordinated mode planner */ + int idForStep; /* status id while stepping */ + } emcmot_internal_t; + +/* error ring buffer access functions */ + extern int emcmotErrorInit(emcmot_error_t * errlog); + extern int emcmotErrorPut(emcmot_error_t * errlog, const char *error); + extern int emcmotErrorPutfv(emcmot_error_t * errlog, const char *fmt, va_list ap); + extern int emcmotErrorPutf(emcmot_error_t * errlog, const char *fmt, ...); + extern int emcmotErrorGet(emcmot_error_t * errlog, char *error); + +#define GET_JOINT_ACTIVE_FLAG(joint) ((joint)->flag & EMCMOT_JOINT_ACTIVE_BIT ? 1 : 0) +#define GET_JOINT_INPOS_FLAG(joint) ((joint)->flag & EMCMOT_JOINT_INPOS_BIT ? 1 : 0) + +#ifdef __cplusplus +} +#endif +#endif /* MOTION_H */ diff --git a/wasm-port/vendor/linuxcnc/src/emc/motion/simple_tp.h b/wasm-port/vendor/linuxcnc/src/emc/motion/simple_tp.h new file mode 100644 index 0000000..26f1a64 --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/motion/simple_tp.h @@ -0,0 +1,107 @@ +/******************************************************************** +* Description: simple_tp.h +* A simple, single axis trajectory planner +* +* Author: +* License: GPL Version 2 +* System: Linux +* +* Copyright (c) 2004 All rights reserved +********************************************************************/ + +/* simple_tp.c and simple_tp.h define a simple, single axis trajectory + planner. It is based on the "free mode trajectory planner" that was + originally written as part of EMC2's control.c, but the code has + been pulled out of control.c and given a somewhat object oriented + API to allow it to be used for both teleop and free mode. +*/ + +#ifndef SIMPLE_TP_H +#define SIMPLE_TP_H + +// stopping criterion: +#define TINY_DP(max_acc,period) (max_acc*period*period*0.001) + +#ifdef __cplusplus +extern "C" { +#endif + + typedef struct simple_tp_t { + double pos_cmd; /* position command */ + double max_vel; /* velocity limit */ + double max_acc; /* acceleration limit */ + double max_jerk; /* jerk limit */ + int enable; /* if zero, motion stops ASAP */ + double curr_pos; /* current position */ + double curr_vel; /* current velocity */ + int active; /* non-zero if motion in progress */ + + double curr_acc; /* current acceleration */ + double curr_jerk; /* current acceleration */ + double last_move_length; /* current acceleration */ + + double last_pos_cmd; + + int use_trapezoid; + double curr_max_vel; + int total_n; + int curr_n; + int n0; + int n1; + int n2; + int n3; + int n4; + int n5; + int n6; + int fix_verr; + double verr; + double vc; + double ve; + double vm; + double jm; + double j2; + double j4; + double v1; + double v2; + double v3; + + double v5; + double v6; + double v7; + + double a1; + double a2; + double a3; + + double a5; + double a6; + double a7; + + double prograss; + + int status; + } simple_tp_t; + +/* I could write a bunch of functions to read and write the first four + structure members, and to read the last three, but that seems silly. +*/ + +/* The update() function does all the work. If 'enable' is true, it + computes a new value of 'curr_pos', which moves toward 'pos_cmd' + while obeying the 'max_vel' and 'max_accel' limits. It also sets + 'active' if movement is in progress, and clears it when motion + stops at the commanded position. The command or either of the + limits can be changed at any time. If 'enable' is false, it + ramps the velocity to zero, then clears 'active' and sets + 'pos_cmd' to match 'curr_pos', to avoid motion the next time it + is enabled. 'period' is the period between calls, in seconds. +*/ + +extern void simple_tp_update(simple_tp_t *tp, double period); +extern void simple_tp_update_normal(simple_tp_t *tp, double period); +extern void simple_scurve_tp_update(simple_tp_t *tp, double period); + +#ifdef __cplusplus +} +#endif +#endif /* SIMPLE_TP_H */ diff --git a/wasm-port/vendor/linuxcnc/src/emc/nml_intf/emcpose.c b/wasm-port/vendor/linuxcnc/src/emc/nml_intf/emcpose.c new file mode 100644 index 0000000..5c58799 --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/nml_intf/emcpose.c @@ -0,0 +1,304 @@ +/******************************************************************** +* Description: emcpose.c +* +* Miscellaneous functions to handle EmcPose operations +* Derived from a work by Fred Proctor & Will Shackleford +* +* Author: Robert W. Ellenberg +* License: GPL Version 2 +* System: Linux +* +* Copyright (c) 2014 All rights reserved. +* +********************************************************************/ + +#include "emcpose.h" +#include +#include + +//#define EMCPOSE_PEDANTIC + +void emcPoseZero(EmcPose * const pos) { +#ifdef EMCPOSE_PEDANTIC + if(!pos) { + return EMCPOSE_ERR_INPUT_MISSING; + } +#endif + + pos->tran.x = 0.0; + pos->tran.y = 0.0; + pos->tran.z = 0.0; + pos->a = 0.0; + pos->b = 0.0; + pos->c = 0.0; + pos->u = 0.0; + pos->v = 0.0; + pos->w = 0.0; +} + + +int emcPoseAdd(EmcPose const * const p1, EmcPose const * const p2, EmcPose * const out) +{ +#ifdef EMCPOSE_PEDANTIC + if (!p1 || !p2) { + return EMCPOSE_ERR_INPUT_MISSING; + } +#endif + + pmCartCartAdd(&p1->tran, &p2->tran, &out->tran); + out->a = p1->a + p2->a; + out->b = p1->b + p2->b; + out->c = p1->c + p2->c; + out->u = p1->u + p2->u; + out->v = p1->v + p2->v; + out->w = p1->w + p2->w; + return EMCPOSE_ERR_OK; +} + +int emcPoseSub(EmcPose const * const p1, EmcPose const * const p2, EmcPose * const out) +{ +#ifdef EMCPOSE_PEDANTIC + if (!p1 || !p2) { + return EMCPOSE_ERR_INPUT_MISSING; + } +#endif + + pmCartCartSub(&p1->tran, &p2->tran, &out->tran); + out->a = p1->a - p2->a; + out->b = p1->b - p2->b; + out->c = p1->c - p2->c; + out->u = p1->u - p2->u; + out->v = p1->v - p2->v; + out->w = p1->w - p2->w; + return EMCPOSE_ERR_OK; + +} + +int emcPoseSelfAdd(EmcPose * const self, EmcPose const * const p2) +{ + return emcPoseAdd(self, p2, self); +} + +int emcPoseSelfSub(EmcPose * const self, EmcPose const * const p2) +{ + return emcPoseSub(self, p2, self); +} + +int emcPoseToPmCartesian(EmcPose const * const pose, + PmCartesian * const xyz, PmCartesian * const abc, PmCartesian * const uvw) +{ + +#ifdef EMCPOSE_PEDANTIC + if (!pose) { + return EMCPOSE_ERR_INPUT_MISSING; + } + if (!xyz | !abc || !uvw) { + return EMCPOSE_ERR_OUTPUT_MISSING; + } +#endif + + //Direct copy of translation struct for xyz + *xyz = pose->tran; + + //Convert ABCUVW axes into 2 pairs of 3D lines + abc->x = pose->a; + abc->y = pose->b; + abc->z = pose->c; + + uvw->x = pose->u; + uvw->y = pose->v; + uvw->z = pose->w; + return EMCPOSE_ERR_OK; +} + + +/** + * Collect PmCartesian elements into 9D EmcPose structure. + */ +int pmCartesianToEmcPose(PmCartesian const * const xyz, + PmCartesian const * const abc, PmCartesian const * const uvw, EmcPose * const pose) +{ +#ifdef EMCPOSE_PEDANTIC + if (!pose) { + return EMCPOSE_ERR_OUTPUT_MISSING; + } + if (!xyz || !abc || !uvw) { + return EMCPOSE_ERR_INPUT_MISSING; + } +#endif + //Direct copy of translation struct for xyz + pose->tran = *xyz; + + pose->a = abc->x; + pose->b = abc->y; + pose->c = abc->z; + + pose->u = uvw->x; + pose->v = uvw->y; + pose->w = uvw->z; + return EMCPOSE_ERR_OK; +} + + +int emcPoseSetXYZ(PmCartesian const * const xyz, EmcPose * const pose) +{ +#ifdef EMCPOSE_PEDANTIC + if (!pose) { + return EMCPOSE_ERR_OUTPUT_MISSING; + } + if (!xyz) { + return EMCPOSE_ERR_INPUT_MISSING; + } +#endif + + pose->tran.x = xyz->x; + pose->tran.y = xyz->y; + pose->tran.z = xyz->z; + return EMCPOSE_ERR_OK; +} + + +int emcPoseSetABC(PmCartesian const * const abc, EmcPose * const pose) +{ +#ifdef EMCPOSE_PEDANTIC + if (!pose) { + return EMCPOSE_ERR_OUTPUT_MISSING; + } + if (!abc) { + return EMCPOSE_ERR_INPUT_MISSING; + } +#endif + + pose->a = abc->x; + pose->b = abc->y; + pose->c = abc->z; + return EMCPOSE_ERR_OK; +} + + +int emcPoseSetUVW(PmCartesian const * const uvw, EmcPose * const pose) +{ +#ifdef EMCPOSE_PEDANTIC + if (!pose) { + return EMCPOSE_ERR_OUTPUT_MISSING; + } + if (!uvw) { + return EMCPOSE_ERR_INPUT_MISSING; + } +#endif + + pose->u = uvw->x; + pose->v = uvw->y; + pose->w = uvw->z; + + return EMCPOSE_ERR_OK; +} + + +int emcPoseGetXYZ(EmcPose const * const pose, PmCartesian * const xyz) +{ +#ifdef EMCPOSE_PEDANTIC + if (!pose) { + return EMCPOSE_ERR_OUTPUT_MISSING; + } + if (!xyz) { + return EMCPOSE_ERR_INPUT_MISSING; + } +#endif + + xyz->x = pose->tran.x; + xyz->y = pose->tran.y; + xyz->z = pose->tran.z; + return EMCPOSE_ERR_OK; +} + + +int emcPoseGetABC(EmcPose const * const pose, PmCartesian * const abc) +{ +#ifdef EMCPOSE_PEDANTIC + if (!pose) { + return EMCPOSE_ERR_OUTPUT_MISSING; + } + if (!abc) { + return EMCPOSE_ERR_INPUT_MISSING; + } +#endif + + abc->x = pose->a; + abc->y = pose->b; + abc->z = pose->c; + return EMCPOSE_ERR_OK; +} + + +int emcPoseGetUVW(EmcPose const * const pose, PmCartesian * const uvw) +{ +#ifdef EMCPOSE_PEDANTIC + if (!pose) { + return EMCPOSE_ERR_OUTPUT_MISSING; + } + if (!uvw) { + return EMCPOSE_ERR_INPUT_MISSING; + } +#endif + + uvw->x = pose->u; + uvw->y = pose->v; + uvw->z = pose->w; + + return EMCPOSE_ERR_OK; +} + + +/** + * Find the magnitude of an EmcPose position, treating it like a single vector. + */ +int emcPoseMagnitude(EmcPose const * const pose, double * const out) { + +#ifdef EMCPOSE_PEDANTIC + if (!pose) { + return EMCPOSE_ERR_INPUT_MISSING; + } + if (!out) { + return EMCPOSE_ERR_OUTPUT_MISSING; + } +#endif + + double mag = 0.0; + mag += pmSq(pose->tran.x); + mag += pmSq(pose->tran.y); + mag += pmSq(pose->tran.z); + mag += pmSq(pose->a); + mag += pmSq(pose->b); + mag += pmSq(pose->c); + mag += pmSq(pose->u); + mag += pmSq(pose->v); + mag += pmSq(pose->w); + mag = pmSqrt(mag); + + *out = mag; + return EMCPOSE_ERR_OK; +} + + +/** + * Return true for a numerically valid pose, or false for an invalid pose (or null pointer). + */ +int emcPoseValid(EmcPose const * const pose) +{ + + if (!pose || + isnan(pose->tran.x) || + isnan(pose->tran.y) || + isnan(pose->tran.z) || + isnan(pose->a) || + isnan(pose->b) || + isnan(pose->c) || + isnan(pose->u) || + isnan(pose->v) || + isnan(pose->w)) { + return 0; + } else { + return 1; + } +} diff --git a/wasm-port/vendor/linuxcnc/src/emc/nml_intf/motion_types.h b/wasm-port/vendor/linuxcnc/src/emc/nml_intf/motion_types.h new file mode 100644 index 0000000..dc7de0b --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/nml_intf/motion_types.h @@ -0,0 +1,25 @@ +#ifndef __LINUXCNC_MOTION_TYPES_H +#define __LINUXCNC_MOTION_TYPES_H +// Copyright 2008, Chris Radek +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. +#define EMC_MOTION_TYPE_TRAVERSE 1 +#define EMC_MOTION_TYPE_FEED 2 +#define EMC_MOTION_TYPE_ARC 3 +#define EMC_MOTION_TYPE_TOOLCHANGE 4 +#define EMC_MOTION_TYPE_PROBING 5 +#define EMC_MOTION_TYPE_INDEXROTARY 6 + +#endif diff --git a/wasm-port/vendor/linuxcnc/src/emc/tp/blendmath.c b/wasm-port/vendor/linuxcnc/src/emc/tp/blendmath.c new file mode 100644 index 0000000..7217be0 --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/tp/blendmath.c @@ -0,0 +1,1871 @@ +/******************************************************************** +* Description: blendmath.c +* Circular arc blend math functions +* +* Author: Robert W. Ellenberg +* License: GPL Version 2 +* System: Linux +* +* Copyright (c) 2014 All rights reserved. +* +* Last change: +********************************************************************/ + +#include +#include + +#include "tc_types.h" +#include "tc.h" +#include "tp_types.h" +#include "spherical_arc.h" +#include "blendmath.h" +#include "tp_debug.h" +#include "../motion/motion.h" +#include "../motion/mot_priv.h" +#include "sp_scurve.h" + +/** @section utilityfuncs Utility functions */ + +/** + * Find the maximum angle allowed between "tangent" segments. + * @param v speed of motion in worst case (i.e. at max feed). + * @param acc magnitude of acceleration allowed during "kink". + * + * Since we are discretized by a timestep, the maximum allowable + * "kink" in a trajectory is bounded by normal acceleration. A small + * kink will effectively be one step along the tightest radius arc + * possible at a given speed. + */ +double findMaxTangentAngle(double v_plan, double acc_limit, double cycle_time) +{ + //Find acc hiccup we're allowed to get + //TODO somewhat redundant with findKinkAccel, should refactor + double acc_margin = BLEND_ACC_RATIO_NORMAL * BLEND_KINK_FACTOR * acc_limit; + double dx = v_plan / cycle_time; + if (dx > 0.0) { + return (acc_margin / dx); + } else { + tp_debug_print(" Velocity or period is negative!\n"); + //Should not happen... + return TP_ANGLE_EPSILON; + } +} + + +/** + * Find the acceleration required to create a specific change in path + * direction, assuming constant speed. + * This determines how much of a "spike" in acceleration will occur due to a + * slight mismatch between tangent directions at the start / end of a segment. + */ +double findKinkAccel(double kink_angle, double v_plan, double cycle_time) +{ + double dx = v_plan / cycle_time; + if (dx > 0.0) { + return (dx * kink_angle); + } else { + rtapi_print_msg(RTAPI_MSG_ERR, "dx < 0 in KinkAccel\n"); + return 0; + } +} + + +/** + * Sign function that returns a valid numerical result for sign(0), rather than NaN. + */ +double fsign(double f) +{ + if (f>0) { + return 1.0; + } else if (f < 0) { + return -1.0; + } else { + //Technically this should be NAN but that's a useless result for tp purposes + return 0; + } +} + +/** negate a value (or not) based on a bool parameter */ +static inline double negate(double f, int neg) +{ + return (neg) ? -f : f; +} + +/** Clip the input at the specified minimum (in place). */ +int clip_min(double * const x, double min) { + if ( *x < min ) { + *x = min; + return 1; + } + return 0; +} + + +/** Clip the input at the specified maximum (in place). */ +int clip_max(double * const x, double max) { + if ( *x > max ) { + *x = max; + return 1; + } + return 0; +} + +/** + * Saturate a value x to be within +/- max. + */ +double saturate(double x, double max) { + if ( x > max ) { + return max; + } + else if ( x < (-max) ) { + return -max; + } + else { + return x; + } +} + +/** + * Saturate a value x to be within max and min. + */ +double bisaturate(double x, double max, double min) { + if ( x > max ) { + return max; + } + else if ( x < min ) { + return min; + } + else { + return x; + } +} + + +/** + * Apply bounds to a value x. + */ +inline double bound(double x, double max, double min) { + if ( x > max ) { + return max; + } + else if ( x < (min) ) { + return min; + } + else { + return x; + } +} + + +/** In-place saturation function */ +int sat_inplace(double * const x, double max) { + if ( *x > max ) { + *x = max; + return 1; + } + else if ( *x < -max ) { + *x = -max; + return -1; + } + return 0; +} + +#if 0 +static int pmCirclePrint(PmCircle const * const circ) { + tp_debug_print(" center = %f %f %f\n", + circ->center.x, + circ->center.y, + circ->center.z); + tp_debug_print(" radius = %f\n", circ->radius); + tp_debug_print(" spiral = %f\n", circ->spiral); + tp_debug_print(" angle = %f\n", circ->angle); + //TODO add other debug data here as needed + return TP_ERR_OK; +} +#endif + +/** + * @section geomfuncs Geometry check functions + */ + +/** + * Calculate the best-fit circle to the spiral segment. + * @param circ spiral to be approximated + * @param base_pt the point about which the circle is fit + * @param u_tan tangent unit vector at the base point of the approximation + * @param[out] center_out displaced center for circular approximation + * @param[out] radius_out adjusted radius + * + * The adjusted center for the circle fit is found by displacing the center + * along the spiral tangent by the spiral coefficient. The adjusted radius is + * the distance between the base point and this new center. + * + */ +static inline int findSpiralApproximation(PmCircle const * const circ, + PmCartesian const * const base_pt, + PmCartesian const * const u_tan, + PmCartesian * const center_out, + double * const radius_out) +{ + double dr = circ->spiral / circ->angle; + + /*tp_debug_print("In findSpiralApproximation\n");*/ + /*tp_debug_print(" dr = %f\n",dr);*/ + /*tp_debug_print(" utan = %f %f %f\n",*/ + /*u_tan->x,*/ + /*u_tan->y,*/ + /*u_tan->z);*/ + pmCartScalMult(u_tan, dr, center_out); + /*tp_debug_print(" circcenter = %f %f %f\n",*/ + /*circ->center.x,*/ + /*circ->center.y,*/ + /*circ->center.z);*/ + pmCartCartAddEq(center_out, &circ->center); + + PmCartesian r_adjust; + pmCartCartSub(base_pt, center_out, &r_adjust); + pmCartMag(&r_adjust, radius_out); + + tp_debug_print(" adjusted center = %f %f %f\n", + center_out->x, + center_out->y, + center_out->z); + + tp_debug_print(" adjusted radius = %f\n", *radius_out); + + return TP_ERR_OK; +} + + +/** + * Calculate the angle to trim from a circle based on the blend geometry. + * + * @param P intersection point + * @param arc_center calculated center of blend arc + * @param center actual center of circle (not spiral approximated center) + * @return trim angle + */ +static inline double findTrimAngle(PmCartesian const * const P, + PmCartesian const * const arc_center, + PmCartesian const * const center) +{ + //Define vectors relative to circle center + PmCartesian u_P; + pmCartCartSub(P, center, &u_P); + pmCartUnitEq(&u_P); + + PmCartesian u_arccenter; + pmCartCartSub(arc_center, center, &u_arccenter); + pmCartUnitEq(&u_arccenter); + + double dot; + pmCartCartDot(&u_arccenter, &u_P, &dot); + double dphi = acos(saturate(dot,1.0)); + tp_debug_print(" dphi = %g\n",dphi); + return dphi; +} + + +/** + * Verify that a blend arc is tangent to a circular arc. + */ +int checkTangentAngle(PmCircle const * const circ, SphericalArc const * const arc, BlendGeom3 const * const geom, BlendParameters const * const param, double cycle_time, int at_end) +{ + (void)geom; + // Debug Information to diagnose tangent issues + PmCartesian u_circ, u_arc; + arcTangent(arc, &u_arc, at_end); + + if (at_end) { + pmCircleTangentVector(circ, 0, &u_circ); + } else { + pmCircleTangentVector(circ, circ->angle, &u_circ); + } + + pmCartUnitEq(&u_arc); + + // Find angle between tangent unit vectors + double dot; + pmCartCartDot(&u_circ, &u_arc, &dot); + double blend_angle = acos(saturate(dot,1.0)); + + // Check against the maximum allowed tangent angle for the given velocity and acceleration + double angle_max = findMaxTangentAngle(param->v_plan, param->a_max, cycle_time); + + tp_debug_print("tangent angle = %f, max = %f\n", + blend_angle, + angle_max); + + tp_debug_print("circ_tan = [%g %g %g]\n", + u_circ.x, + u_circ.y, + u_circ.z); + tp_debug_print("arc_tan = [%g %g %g]\n", + u_arc.x, + u_arc.y, + u_arc.z); + + PmCartesian diff; + pmCartCartSub(&u_arc,&u_circ,&diff); + tp_debug_print("diff = [%g %g %g]\n", + diff.x, + diff.y, + diff.z); + + if (blend_angle > angle_max) { + tp_debug_print("angle too large\n"); + return TP_ERR_FAIL; + } + + return TP_ERR_OK; +} + + +/** + * Checks if two UNIT vectors are parallel to the given angle tolerance (in radians). + * @warning tol depends on the small angle approximation and will not be + * accurate for angles larger than about 10 deg. This function is meant for + * small tolerances! + */ +int pmCartCartParallel(PmCartesian const * const u1, + PmCartesian const * const u2, + double tol) +{ + double d_diff; + { + PmCartesian u_diff; + pmCartCartSub(u1, u2, &u_diff); + pmCartMagSq(&u_diff, &d_diff); + } + + tp_debug_json_start(pmCartCartParallel); + tp_debug_json_double(d_diff); + tp_debug_json_end(); + + return d_diff < tol; +} + +/** + * Checks if two UNIT vectors are anti-parallel to the given angle tolerance (in radians). + * @warning tol depends on the small angle approximation and will not be + * accurate for angles larger than about 10 deg. This function is meant for + * small tolerances! + */ +int pmCartCartAntiParallel(PmCartesian const * const u1, + PmCartesian const * const u2, + double tol) +{ + double d_sum; + { + PmCartesian u_sum; + pmCartCartAdd(u1, u2, &u_sum); + pmCartMagSq(&u_sum, &d_sum); + } + + tp_debug_json_start(pmCartCartAntiParallel); + tp_debug_json_double(d_sum); + tp_debug_json_end(); + + return d_sum < tol; +} + + +/** + * Check if two cartesian vectors are parallel or anti-parallel + * The input tolerance specifies what the maximum angle between the + * lines containing two vectors is. Note that vectors pointing in + * opposite directions are still considered parallel, since their + * containing lines are parallel. + * @param u1 input unit vector 1 + * @param u2 input unit vector 2 + * @pre BOTH u1 and u2 must be unit vectors or calculation may be skewed. + */ +int pmUnitCartsColinear(PmCartesian const * const u1, + PmCartesian const * const u2) +{ + return pmCartCartParallel(u1, u2, TP_ANGLE_EPSILON_SQ) || pmCartCartAntiParallel(u1, u2, TP_ANGLE_EPSILON_SQ); +} + + +/** + * Somewhat redundant function to calculate the segment intersection angle. + * The intersection angle is half of the supplement of the "divergence" angle + * between unit vectors. If two unit vectors are pointing in the same + * direction, then the intersection angle is PI/2. This is based on the + * simple_tp formulation for tolerances. + */ +int findIntersectionAngle(PmCartesian const * const u1, + PmCartesian const * const u2, double * const theta) +{ + double dot; + pmCartCartDot(u1, u2, &dot); + + if (dot > 1.0 || dot < -1.0) { + tp_debug_print("dot product %.16g outside domain of acos! u1 = %.16g %.16g %.16g, u2 = %.16g %.16g %.16g\n", + dot, + u1->x, + u1->y, + u1->z, + u2->x, + u2->y, + u2->z); + sat_inplace(&dot,1.0); + } + + *theta = acos(-dot)/2.0; + return TP_ERR_OK; +} + + +/** Calculate the minimum of the three values in a PmCartesian. */ +double pmCartMin(PmCartesian const * const in) +{ + return fmin(fmin(in->x,in->y),in->z); +} + + +/** + * Calculate the diameter of a circle incscribed on a central cross section of a 3D + * rectangular prism. + * + * @param normal normal direction of plane slicing prism. + * @param extents distance from center to one corner of the prism. + * @param diameter diameter of inscribed circle on cross section. + * + */ +int calculateInscribedDiameter(PmCartesian const * const normal, + PmCartesian const * const bounds, double * const diameter) +{ + if (!normal ) { + return TP_ERR_MISSING_INPUT; + } + + double n_mag; + pmCartMagSq(normal, &n_mag); + double mag_err = fabs(1.0 - n_mag); + if (mag_err > pmSqrt(TP_POS_EPSILON)) { + /*rtapi_print_msg(RTAPI_MSG_ERR,"normal vector <%.12g,%.12f,%.12f> has magnitude error = %e\n",*/ + /*normal->x,*/ + /*normal->y,*/ + /*normal->z,*/ + /*mag_err);*/ + return TP_ERR_FAIL; + } + + PmCartesian planar_x,planar_y,planar_z; + + //Find perpendicular component of unit directions + // FIXME Assumes normal is unit length + + /* This section projects the X / Y / Z unit vectors onto the plane + * containing the motions. The operation is done "backwards" here due to a + * quirk with posemath. + * + */ + pmCartScalMult(normal, -normal->x, &planar_x); + pmCartScalMult(normal, -normal->y, &planar_y); + pmCartScalMult(normal, -normal->z, &planar_z); + + planar_x.x += 1.0; + planar_y.y += 1.0; + planar_z.z += 1.0; + + pmCartAbs(&planar_x, &planar_x); + pmCartAbs(&planar_y, &planar_y); + pmCartAbs(&planar_z, &planar_z); + + // Crude way to prevent divide-by-zero-error + planar_x.x = fmax(planar_x.x,TP_POS_EPSILON); + planar_y.y = fmax(planar_y.y,TP_POS_EPSILON); + planar_z.z = fmax(planar_z.z,TP_POS_EPSILON); + + double x_scale, y_scale, z_scale; + pmCartMag(&planar_x, &x_scale); + pmCartMag(&planar_y, &y_scale); + pmCartMag(&planar_z, &z_scale); + + double x_extent=0, y_extent=0, z_extent=0; + if (bounds->x != 0) { + x_extent = bounds->x / x_scale; + } + if (bounds->y != 0) { + y_extent = bounds->y / y_scale; + } + if (bounds->z != 0) { + z_extent = bounds->z / z_scale; + } + + // Find the highest value to start from + *diameter = fmax(fmax(x_extent, y_extent),z_extent); + + // Only for active axes, find the minimum extent + if (bounds->x != 0) { + *diameter = fmin(*diameter, x_extent); + } + if (bounds->y != 0) { + *diameter = fmin(*diameter, y_extent); + } + if (bounds->z != 0) { + *diameter = fmin(*diameter, z_extent); + } + + return TP_ERR_OK; +} + + + +int findAccelScale(PmCartesian const * const acc, + PmCartesian const * const bounds, + PmCartesian * const scale) +{ + if (!acc || !bounds ) { + return TP_ERR_MISSING_INPUT; + } + + if (!scale ) { + return TP_ERR_MISSING_OUTPUT; + } + + // Find the scale of acceleration vs. machine accel bounds + if (bounds->x != 0) { + scale->x = fabs(acc->x / bounds->x); + } else { + scale->x = 0; + } + if (bounds->y != 0) { + scale->y = fabs(acc->y / bounds->y); + } else { + scale->y = 0; + } + + if (bounds->z != 0) { + scale->z = fabs(acc->z / bounds->z); + } else { + scale->z = 0; + } + + return TP_ERR_OK; +} + + + + +/** Find real roots of a quadratic equation in standard form. */ +int quadraticFormula(double A, double B, double C, double * const root0, + double * const root1) +{ + double disc = pmSq(B) - 4.0 * A * C; + if (disc < 0) { + tp_debug_print("discriminant %.12g < 0, A=%.12g, B=%.12g,C=%.12g\n", disc, A, B, C); + return TP_ERR_FAIL; + } + double t1 = pmSqrt(disc); + if (root0) { + *root0 = ( -B + t1) / (2.0 * A); + } + if (root1) { + *root1 = ( -B - t1) / (2.0 * A); + } + return TP_ERR_OK; +} + +/** + * @section blending blend math functions + */ + +/** + * Setup common geom parameters based on trajectory segments. + * This function populates the geom structure and "input" fields of + * the blend parameter structure. It returns an error if the segments + * are not coplanar, or if one or both segments is not a circular arc. + * + * @param geom Stores simplified geometry used to calculate blend params. + * @param prev_tc first linear move to blend + * @param tc second linear move to blend + */ +int blendGeom3Init(BlendGeom3 * const geom, + TC_STRUCT const * const prev_tc, + TC_STRUCT const * const tc) +{ + geom->v_max1 = prev_tc->maxvel; + geom->v_max2 = tc->maxvel; + + // Get tangent unit vectors to each arc at the intersection point + int res_u1 = tcGetEndTangentUnitVector(prev_tc, &geom->u_tan1); + int res_u2 = tcGetStartTangentUnitVector(tc, &geom->u_tan2); + + // Initialize u1 and u2 by assuming they match the tangent direction + geom->u1 = geom->u_tan1; + geom->u2 = geom->u_tan2; + + int res_intersect = tcGetIntersectionPoint(prev_tc, tc, &geom->P); + + tp_debug_print("Intersection point P = %f %f %f\n", + geom->P.x, + geom->P.y, + geom->P.z); + + // Find angle between tangent vectors + int res_angle = findIntersectionAngle(&geom->u_tan1, + &geom->u_tan2, + &geom->theta_tan); + + // Test for intersection angle errors + if(PM_PI / 2.0 - geom->theta_tan < TP_ANGLE_EPSILON) { + tp_debug_print("Intersection angle too close to pi/2, can't compute normal\n"); + return TP_ERR_TOLERANCE; + } + + if(geom->theta_tan < TP_ANGLE_EPSILON) { + tp_debug_print("Intersection angle too small for arc fit\n"); + return TP_ERR_TOLERANCE; + } + + blendCalculateNormals3(geom); + + return res_u1 | + res_u2 | + res_intersect | + res_angle; +} + + +/** + * Initialize common fields in parameters structure. + * + * @param geom Stores simplified geometry used to calculate blend params. + * @param param Abstracted parameters for blending calculations + * @param acc_bound maximum X, Y, Z machine acceleration + * @param vel_bound maximum X, Y, Z machine velocity + * @param maxFeedScale maximum allowed feed override (set in INI) + */ +int blendParamKinematics(BlendGeom3 * const geom, + BlendParameters * const param, + TC_STRUCT const * const prev_tc, + TC_STRUCT const * const tc, + PmCartesian const * const acc_bound, + PmCartesian const * const vel_bound, + double maxFeedScale) +{ + + // KLUDGE: common operations, but not exactly kinematics + param->phi = (PM_PI - param->theta * 2.0); + + double nominal_tolerance; + tcFindBlendTolerance(prev_tc, tc, ¶m->tolerance, &nominal_tolerance); + + // Calculate max acceleration based on plane containing lines + int res_dia = calculateInscribedDiameter(&geom->binormal, acc_bound, ¶m->a_max); + + // Store max normal acceleration + param->a_n_max = param->a_max * BLEND_ACC_RATIO_NORMAL; + tp_debug_print("a_max = %f, a_n_max = %f\n", param->a_max, + param->a_n_max); + + // Find the nominal velocity for the blend segment with no overrides + double v_req_prev = tcGetMaxTargetVel(prev_tc, 1.0); + double v_req_this = tcGetMaxTargetVel(tc, 1.0); + tp_debug_print("vr_prev = %f, vr_this = %f\n", v_req_prev, v_req_this); + param->v_req = fmax(v_req_prev, v_req_this); + + // Find the worst-case velocity we should reach for either segment + param->v_goal = fmax(tcGetMaxTargetVel(prev_tc, maxFeedScale), + tcGetMaxTargetVel(tc, maxFeedScale)); + + // Calculate the maximum planar velocity + double v_planar_max = 0; + //FIXME sloppy handling of return value + res_dia |= calculateInscribedDiameter(&geom->binormal, vel_bound, &v_planar_max); + tp_debug_print("v_planar_max = %f\n", v_planar_max); + + // Clip the angle at a reasonable value (less than 90 deg), to prevent div by zero + double phi_effective = fmin(param->phi, PM_PI * 0.49); + + // Copy over maximum velocities, clipping velocity to place altitude within base + double v_max1 = fmin(prev_tc->maxvel, tc->maxvel / cos(phi_effective)); + double v_max2 = fmin(tc->maxvel, prev_tc->maxvel / cos(phi_effective)); + + tp_debug_print("v_max1 = %f, v_max2 = %f\n", v_max1, v_max2); + + // Get "altitude" + double v_area = v_max1 * v_max2 / 2.0 * sin(param->phi); + tp_debug_print("phi = %f\n", param->phi); + tp_debug_print("v_area = %f\n", v_area); + + // Get "base" of triangle + PmCartesian tmp1, tmp2, diff; + pmCartScalMult(&geom->u1, v_max1, &tmp1); + pmCartScalMult(&geom->u2, v_max2, &tmp2); + pmCartCartSub(&tmp2, &tmp1, &diff); + double base; + pmCartMag(&diff, &base); + tp_debug_print("v_base = %f\n", base); + + double v_max_alt = 2.0 * v_area / base; + + // Can't do altitude-based velocity calculation if we have arcs + if (prev_tc->motion_type != TC_LINEAR || tc->motion_type != TC_LINEAR) { + v_max_alt = 0.0; + } + + tp_debug_print("v_max_alt = %f\n", v_max_alt); + double v_max = fmax(v_max_alt, v_planar_max); + + tp_debug_print("v_max = %f\n", v_max); + param->v_goal = fmin(param->v_goal, v_max); + + tp_debug_print("v_goal = %f, max scale = %f\n", param->v_goal, maxFeedScale); + + return res_dia; +} + +/** + * Setup blend parameters based on a line and an arc. + * This function populates the geom structure and "input" fields of + * the blend parameter structure. It returns an error if the segments + * are not coplanar, or if one or both segments is not a circular arc. + * + * @param geom Stores simplified geometry used to calculate blend params. + * @param param Abstracted parameters for blending calculations + * @param prev_tc first linear move to blend + * @param tc second linear move to blend + * @param acc_bound maximum X, Y, Z machine acceleration + * @param vel_bound maximum X, Y, Z machine velocity + * @param maxFeedScale maximum allowed feed override (set in INI) + */ +int blendInit3FromLineArc(BlendGeom3 * const geom, BlendParameters * const param, + TC_STRUCT const * const prev_tc, + TC_STRUCT const * const tc, + PmCartesian const * const acc_bound, + PmCartesian const * const vel_bound, + double maxFeedScale) +{ + + if (tc->motion_type != TC_CIRCULAR || prev_tc->motion_type != TC_LINEAR) { + return TP_ERR_INPUT_TYPE; + } + + int res_init = blendGeom3Init(geom, prev_tc, tc); + if (res_init != TP_ERR_OK) { + return res_init; + } + + //Fit spiral approximation + findSpiralApproximation(&tc->coords.circle.xyz, + &geom->P, + &geom->u_tan2, + &geom->center2, + &geom->radius2); + // Handle convexity + param->convex2 = arcConvexTest(&geom->center2, &geom->P, &geom->u_tan1, true); + tp_debug_print("circ2 convex: %d\n", + param->convex2); + + //Identify max angle for first arc by blend limits + // TODO better name? + double blend_angle_2 = param->convex2 ? geom->theta_tan : PM_PI / 2.0; + + param->phi2_max = fmin(tc->coords.circle.xyz.angle / 3.0, blend_angle_2); + param->theta = geom->theta_tan; + + if (param->convex2) { + PmCartesian blend_point; + pmCirclePoint(&tc->coords.circle.xyz, + param->phi2_max / 2.0, + &blend_point); + //Create new unit vector based on secant line + // Direction is away from P (at start of segment) + pmCartCartSub(&blend_point, &geom->P, &geom->u2); + pmCartUnitEq(&geom->u2); + //Reduce theta proportionally to the angle between the secant and the normal + param->theta = fmin(param->theta, geom->theta_tan - param->phi2_max / 4.0); + } + + tp_debug_print("phi2_max = %f\n", param->phi2_max); + blendGeom3Print(geom); + + // Check that we're not below the minimum intersection angle (making too tight an arc) + // FIXME make this an INI setting? + const double theta_min = PM_PI / 6.0; + if (param->theta < theta_min) { + tp_debug_print("theta = %f < min %f, aborting arc...\n", + param->theta, + theta_min); + } + + tp_debug_print("theta = %f\n", param->theta); + + param->phi = (PM_PI - param->theta * 2.0); + + param->L1 = fmin(prev_tc->target, prev_tc->nominal_length / 2.0); + + if (param->convex2) { + //use half of the length of the chord + param->L2 = sin(param->phi2_max/4.0) * geom->radius2; + } else { + param->L2 = param->phi2_max * geom->radius2; + } + + tp_debug_print("L1 = %f, L2 = %f\n", param->L1, param->L2); + + // Setup common parameters + int res_kin = blendParamKinematics(geom, + param, + prev_tc, + tc, + acc_bound, + vel_bound, + maxFeedScale); + + return res_kin; +} + +int blendInit3FromArcLine(BlendGeom3 * const geom, BlendParameters * const param, + TC_STRUCT const * const prev_tc, + TC_STRUCT const * const tc, + PmCartesian const * const acc_bound, + PmCartesian const * const vel_bound, + double maxFeedScale) +{ + + if (tc->motion_type != TC_LINEAR || prev_tc->motion_type != TC_CIRCULAR) { + return TP_ERR_INPUT_TYPE; + } + + int res_init = blendGeom3Init(geom, prev_tc, tc); + if (res_init != TP_ERR_OK) { + return res_init; + } + + findSpiralApproximation(&prev_tc->coords.circle.xyz, + &geom->P, + &geom->u_tan1, + &geom->center1, + &geom->radius1); + + param->convex1 = arcConvexTest(&geom->center1, &geom->P, &geom->u_tan2, false); + tp_debug_print("circ1 convex: %d\n", + param->convex1); + + //Identify max angle for first arc by blend limits + // TODO better name? + double blend_angle_1 = param->convex1 ? geom->theta_tan : PM_PI / 2.0; + + param->phi1_max = fmin(prev_tc->coords.circle.xyz.angle * 2.0 / 3.0, blend_angle_1); + param->theta = geom->theta_tan; + + // Build the correct unit vector for the linear approximation + if (param->convex1) { + PmCartesian blend_point; + pmCirclePoint(&prev_tc->coords.circle.xyz, + prev_tc->coords.circle.xyz.angle - param->phi1_max / 2.0 , + &blend_point); + //Create new unit vector based on secant line + // Direction is toward P (at end of segment) + pmCartCartSub(&geom->P, &blend_point, &geom->u1); + pmCartUnitEq(&geom->u1); + + //Reduce theta proportionally to the angle between the secant and the normal + param->theta = fmin(param->theta, geom->theta_tan - param->phi1_max / 4.0); + } + + blendGeom3Print(geom); + tp_debug_print("phi1_max = %f\n", param->phi1_max); + + // Check that we're not below the minimum intersection angle (making too tight an arc) + // FIXME make this an INI setting? + const double theta_min = PM_PI / 6.0; + if (param->theta < theta_min) { + tp_debug_print("theta = %f < min %f, aborting arc...\n", + param->theta, + theta_min); + } + + tp_debug_print("theta = %f\n", param->theta); + + // Use end radius here + param->L1 = param->phi1_max * (geom->radius1); + param->L2 = tc->nominal_length / 2.0; + + if (param->convex1) { + //use half of the length of the chord + param->L1 = sin(param->phi1_max/4.0) * geom->radius1; + } + tp_debug_print("L1 = %f, L2 = %f\n", param->L1, param->L2); + + // Setup common parameters + int res_kin = blendParamKinematics(geom, + param, + prev_tc, + tc, + acc_bound, + vel_bound, + maxFeedScale); + + return res_kin; +} + + +/** + * Setup blend parameters based on two circular arc segments. + * This function populates the geom structure and "input" fields of + * the blend parameter structure. It returns an error if the segments + * are not coplanar, or if one or both segments is not a circular arc. + * + * @param geom Stores simplified geometry used to calculate blend params. + * @param param Abstracted parameters for blending calculations + * @param prev_tc first linear move to blend + * @param tc second linear move to blend + * @param acc_bound maximum X, Y, Z machine acceleration + * @param vel_bound maximum X, Y, Z machine velocity + * @param maxFeedScale maximum allowed feed override (set in INI) + */ +int blendInit3FromArcArc(BlendGeom3 * const geom, BlendParameters * const param, + TC_STRUCT const * const prev_tc, + TC_STRUCT const * const tc, + PmCartesian const * const acc_bound, + PmCartesian const * const vel_bound, + double maxFeedScale) +{ + if (tc->motion_type != TC_CIRCULAR || prev_tc->motion_type != TC_CIRCULAR) { + return TP_ERR_FAIL; + } + + int res_init = blendGeom3Init(geom, prev_tc, tc); + if (res_init != TP_ERR_OK) { + return res_init; + } + + findSpiralApproximation(&prev_tc->coords.circle.xyz, + &geom->P, + &geom->u_tan1, + &geom->center1, + &geom->radius1); + + findSpiralApproximation(&tc->coords.circle.xyz, + &geom->P, + &geom->u_tan2, + &geom->center2, + &geom->radius2); + + + //Do normal calculation here since we need this information for accel / vel limits + blendCalculateNormals3(geom); + + // Get intersection point from circle start + pmCirclePoint(&tc->coords.circle.xyz, 0.0, &geom->P); + tp_debug_print("Intersection point P = %f %f %f\n", + geom->P.x, + geom->P.y, + geom->P.z); + + param->convex1 = arcConvexTest(&geom->center1, &geom->P, &geom->u_tan2, false); + param->convex2 = arcConvexTest(&geom->center2, &geom->P, &geom->u_tan1, true); + tp_debug_print("circ1 convex: %d, circ2 convex: %d\n", + param->convex1, + param->convex2); + + //Identify max angle for first arc by blend limits + // TODO better name? + double blend_angle_1 = param->convex1 ? geom->theta_tan : PM_PI / 2.0; + double blend_angle_2 = param->convex2 ? geom->theta_tan : PM_PI / 2.0; + + param->phi1_max = fmin(prev_tc->coords.circle.xyz.angle * 2.0 / 3.0, blend_angle_1); + param->phi2_max = fmin(tc->coords.circle.xyz.angle / 3.0, blend_angle_2); + + param->theta = geom->theta_tan; + + // Build the correct unit vector for the linear approximation + if (param->convex1) { + PmCartesian blend_point; + pmCirclePoint(&prev_tc->coords.circle.xyz, + prev_tc->coords.circle.xyz.angle - param->phi1_max / 2.0, + &blend_point); + //Create new unit vector based on secant line + // Direction is toward P (at end of segment) + pmCartCartSub(&geom->P, &blend_point, &geom->u1); + pmCartUnitEq(&geom->u1); + + //Reduce theta proportionally to the angle between the secant and the normal + param->theta = fmin(param->theta, geom->theta_tan - param->phi1_max / 4.0); + + } + + if (param->convex2) { + PmCartesian blend_point; + pmCirclePoint(&tc->coords.circle.xyz, + param->phi2_max / 2.0, + &blend_point); + //Create new unit vector based on secant line + // Direction is away from P (at start of segment) + pmCartCartSub(&blend_point, &geom->P, &geom->u2); + pmCartUnitEq(&geom->u2); + + //Reduce theta proportionally to the angle between the secant and the normal + param->theta = fmin(param->theta, geom->theta_tan - param->phi2_max / 4.0); + } + blendGeom3Print(geom); + + + // Check that we're not below the minimum intersection angle (making too tight an arc) + // FIXME make this an INI setting? + const double theta_min = PM_PI / 12.0; + if (param->theta < theta_min) { + tp_debug_print("theta = %f < min %f, aborting arc...\n", + param->theta, + theta_min); + return TP_ERR_FAIL; + } + + tp_debug_print("theta = %f\n", param->theta); + + param->phi = (PM_PI - param->theta * 2.0); + + param->L1 = param->phi1_max * geom->radius1; + param->L2 = param->phi2_max * geom->radius2; + + if (param->convex1) { + //use half of the length of the chord + param->L1 = sin(param->phi1_max/4.0) * geom->radius1; + } + if (param->convex2) { + //use half of the length of the chord + param->L2 = sin(param->phi2_max/4.0) * geom->radius2; + } + tp_debug_print("L1 = %f, L2 = %f\n", param->L1, param->L2); + tp_debug_print("phi1_max = %f\n",param->phi1_max); + tp_debug_print("phi2_max = %f\n",param->phi2_max); + + // Setup common parameters + int res_kin = blendParamKinematics(geom, + param, + prev_tc, + tc, + acc_bound, + vel_bound, + maxFeedScale); + + return res_kin; +} + +/** + * Setup blend parameters based on two linear segments. + * This function populates the geom structure and "input" fields of the blend parameter structure based. + * @param geom Stores simplified geometry used to calculate blend params. + * @param param Abstracted parameters for blending calculations + * @param prev_tc first linear move to blend + * @param tc second linear move to blend + * @param acc_bound maximum X, Y, Z machine acceleration + * @param vel_bound maximum X, Y, Z machine velocity + * @param maxFeedScale maximum allowed feed override (set in INI) + */ +int blendInit3FromLineLine(BlendGeom3 * const geom, BlendParameters * const param, + TC_STRUCT const * const prev_tc, + TC_STRUCT const * const tc, + PmCartesian const * const acc_bound, + PmCartesian const * const vel_bound, + double maxFeedScale) +{ + + if (tc->motion_type != TC_LINEAR || prev_tc->motion_type != TC_LINEAR) { + return TP_ERR_FAIL; + } + + int res_init = blendGeom3Init(geom, prev_tc, tc); + if (res_init != TP_ERR_OK) { + return res_init; + } + + param->theta = geom->theta_tan; + + tp_debug_print("theta = %f\n", param->theta); + + param->phi = (PM_PI - param->theta * 2.0); + + blendGeom3Print(geom); + + //Nominal length restriction prevents gobbling too much of parabolic blends + param->L1 = fmin(prev_tc->target, prev_tc->nominal_length * BLEND_DIST_FRACTION); + param->L2 = tc->target * BLEND_DIST_FRACTION; + tp_debug_print("prev. nominal length = %f, next nominal_length = %f\n", + prev_tc->nominal_length, tc->nominal_length); + tp_debug_print("L1 = %f, L2 = %f\n", param->L1, param->L2); + + // Setup common parameters + int res_kin = blendParamKinematics(geom, + param, + prev_tc, + tc, + acc_bound, + vel_bound, + maxFeedScale); + + return res_kin; +} + + +/** + * Calculate plane normal and binormal based on unit direction vectors. + */ +int blendCalculateNormals3(BlendGeom3 * const geom) +{ + + int err_cross = pmCartCartCross(&geom->u_tan1, + &geom->u_tan2, + &geom->binormal); + int err_unit_b = pmCartUnitEq(&geom->binormal); + + tp_debug_print("binormal = [%f %f %f]\n", geom->binormal.x, + geom->binormal.y, + geom->binormal.z); + + pmCartCartSub(&geom->u_tan2, &geom->u_tan1, &geom->normal); + int err_unit_n = pmCartUnitEq(&geom->normal); + + tp_debug_print("normal = [%f %f %f]\n", geom->normal.x, + geom->normal.y, + geom->normal.z); + return (err_cross || err_unit_b || err_unit_n); +} + +/** + * Compute blend parameters based on line data. + * Blend arc parameters such as radius and velocity are calculated here. These + * parameters are later used to create the actual arc geometry in other + * functions. + */ +int blendComputeParameters(BlendParameters * const param) +{ + + // Find maximum distance h from arc center to intersection point + double h_tol = param->tolerance / (1.0 - sin(param->theta)); + + // Find maximum distance along lines allowed by tolerance + double d_tol = cos(param->theta) * h_tol; + tp_debug_print(" d_tol = %f\n", d_tol); + + // Find minimum distance by blend length constraints + double d_lengths = fmin(param->L1, param->L2); + double d_geom = fmin(d_lengths, d_tol); + // Find radius from the limiting length + double R_geom = tan(param->theta) * d_geom; + + // Find maximum velocity allowed by accel and radius + double v_normal; + + if(GET_TRAJ_PLANNER_TYPE() == 1){ + v_normal = findSCurveVPeak(param->a_n_max, emcmotStatus->jerk, R_geom); + }else{ + v_normal = pmSqrt(param->a_n_max * R_geom); + } + + param->v_plan = fmin(v_normal, param->v_goal); + + /*Get the limiting velocity of the equivalent parabolic blend. We use the + * time it would take to do a "stock" parabolic blend as a metric for how + * much of the segment to consume. A long segment will have a high + * "triangle" velocity, so the radius will only be as large as is needed to + * reach the cornering speed. A short segment will have a low triangle + * velocity, much lower than the actual curvature limit, which can be used + * to calculate an equivalent blend radius. + * */ + double a_parabolic = param->a_max * 0.5; + double v_triangle = pmSqrt(2.0 * a_parabolic * d_geom); + double t_blend = fmin(v_triangle, param->v_plan) / (a_parabolic); + double s_blend = t_blend * param->v_plan; + double R_blend = fmin(s_blend / param->phi, R_geom); //Clamp by limiting radius + + // Calculate minimum radius needed to keep jerk within limits at v_plan + // For circular motion: j = v³/R² (worst case at corner transitions) + // Therefore: R_min = v^(3/2) / sqrt(j_max) + double R_jerk_min = 0.0; + if (GET_TRAJ_PLANNER_TYPE() == 1 && emcmotStatus->jerk > TP_POS_EPSILON) { + // R_min = v^(3/2) / sqrt(j) + double v_32 = pmSqrt(param->v_plan) * param->v_plan; // v^(3/2) + double j_sqrt = pmSqrt(emcmotStatus->jerk); + R_jerk_min = v_32 / j_sqrt; + + tp_debug_print("R_jerk_min = %f (for v=%f, j=%f)\n", + R_jerk_min, param->v_plan, emcmotStatus->jerk); + } + + // Calculate radius from acceleration constraint + double R_accel = pmSq(param->v_plan) / param->a_n_max; + + // Apply jerk constraint - use the larger of R_blend and R_jerk_min + double R_min = fmax(R_blend, R_jerk_min); + + // Final radius must satisfy both acceleration and minimum (blend/jerk) constraints + param->R_plan = fmax(R_accel, R_min); + + // Calculate arc length + param->s_arc = param->R_plan * param->phi; + + tp_debug_print("R_accel=%f, R_blend=%f, R_jerk_min=%f, R_plan=%f\n", + R_accel, R_blend, R_jerk_min, param->R_plan); + + // Note: Velocity jerk limiting for blend arcs is now handled uniformly + // in tcUpdateArcLimits() during segment finalization, along with TC_CIRCULAR arcs. + + param->d_plan = param->R_plan / tan(param->theta); + + tp_debug_print("v_plan = %f\n", param->v_plan); + + /* "Actual" velocity means the velocity when feed override is 1.0. Recall + * that v_plan may be greater than v_req by the max feed override. If our + * worst-case planned velocity is higher than the requested velocity, then + * clip at the requested velocity. This allows us to increase speed above + * the feed override limits. + */ + if (param->v_plan > param->v_req) { + param->v_actual = param->v_req; + } else { + param->v_actual = param->v_plan; + } + + if (param->R_plan < TP_POS_EPSILON) { + tp_debug_print("#Blend radius too small, aborting arc\n"); + return TP_ERR_FAIL; + } + + if (param->s_arc < TP_MIN_ARC_LENGTH) { + tp_debug_print("#Blend arc length too small, aborting arc\n"); + return TP_ERR_FAIL; + } + return TP_ERR_OK; +} + + +/** Check if the previous line segment will be consumed based on the blend arc parameters. */ +int blendCheckConsume(BlendParameters * const param, + BlendPoints3 const * const points, + TC_STRUCT const * const prev_tc, int gap_cycles) +{ + //Initialize values + param->consume = 0; + param->line_length = 0; + if (!prev_tc) { + return -1; + } + + if (prev_tc->motion_type != TC_LINEAR) { + return 0; + } + + //Check for segment length limits + double L_prev = prev_tc->target - points->trim1; + double prev_seg_time = L_prev / param->v_plan; + + bool can_consume = tcCanConsume(prev_tc); + param->consume = (prev_seg_time < gap_cycles * prev_tc->cycle_time && can_consume); + if (param->consume) { + tp_debug_print("consuming prev line, L_prev = %g\n", + L_prev); + param->line_length = L_prev; + } + return 0; +} + + +/** + * Compute spherical arc points based on blend arc data. + * Once blend parameters are computed, the three arc points are calculated + * here. + */ +int blendFindPoints3(BlendPoints3 * const points, BlendGeom3 const * const geom, + BlendParameters const * const param) +{ + // Find center of blend arc along normal vector + double center_dist = param->R_plan / sin(param->theta); + tp_debug_print("center_dist = %f\n", center_dist); + + pmCartScalMult(&geom->normal, center_dist, &points->arc_center); + pmCartCartAddEq(&points->arc_center, &geom->P); + tp_debug_print("arc_center = %f %f %f\n", + points->arc_center.x, + points->arc_center.y, + points->arc_center.z); + + // Start point is d_plan away from intersection P in the + // negative direction of u1 + pmCartScalMult(&geom->u1, -param->d_plan, &points->arc_start); + pmCartCartAddEq(&points->arc_start, &geom->P); + tp_debug_print("arc_start = %f %f %f\n", + points->arc_start.x, + points->arc_start.y, + points->arc_start.z); + + // End point is d_plan away from intersection P in the + // positive direction of u1 + pmCartScalMult(&geom->u2, param->d_plan, &points->arc_end); + pmCartCartAddEq(&points->arc_end, &geom->P); + tp_debug_print("arc_end = %f %f %f\n", + points->arc_end.x, + points->arc_end.y, + points->arc_end.z); + + //For line case, just copy over d_plan since it's the same + points->trim1 = param->d_plan; + points->trim2 = param->d_plan; + + return TP_ERR_OK; +} + + +/** + * Take results of line blend calculation and project onto circular arc and line + */ +int blendLineArcPostProcess(BlendPoints3 * const points, BlendPoints3 const * const points_in, + BlendParameters * const param, BlendGeom3 const * const geom, + PmCartLine const * const line1, PmCircle const * const circ2) +{ + (void)points_in; + (void)line1; + (void)circ2; + + // Define distances from actual center to circle centers + double d2 = negate(param->R_plan, param->convex2) + geom->radius2; + tp_debug_print("d2 = %f\n", d2); + + //Get unit vector normal to line in plane, towards arc center + PmCartesian n1; + pmCartCartCross(&geom->binormal, &geom->u1, &n1); + pmCartUnitEq(&n1); + + tp_debug_print("n1 = %f %f %f\n", + n1.x, + n1.y, + n1.z); + + PmCartesian r_PC2; + pmCartCartSub(&geom->center2, &geom->P, &r_PC2); + + double c2_u,c2_n; //Components of C2-P on u1 and n1 + pmCartCartDot(&r_PC2, &geom->u1, &c2_u); + pmCartCartDot(&r_PC2, &n1, &c2_n); + + tp_debug_print("c2_u = %f, c2_n = %f\n", + c2_u, + c2_n); + + double d_L; // badly named distance along line to intersection + double A = 1; + double B = 2.0 * c2_u; + double C = pmSq(c2_u) - pmSq(d2) + pmSq(param->R_plan - c2_n); + double root0,root1; + int res_dist = quadraticFormula(A, B, C, &root0, &root1); + if (res_dist) { + return TP_ERR_FAIL; + } + + tp_debug_print("root0 = %f, root1 = %f\n", root0, + root1); + d_L = fmin(fabs(root0),fabs(root1)); + if (d_L < 0) { + tp_debug_print("d_L can't be < 0, aborting...\n"); + return TP_ERR_FAIL; + } + + PmCartesian C_u, C_n; + + pmCartScalMult(&geom->u1, -d_L, &C_u); + pmCartScalMult(&n1, param->R_plan, &C_n); + + PmCartesian r_PC; + //Continue with correct solution, get actual center + pmCartCartAdd(&C_u, &C_n, &r_PC); + pmCartCartAdd(&geom->P, &r_PC, &points->arc_center); + tp_debug_print("arc center = %f %f %f\n", + points->arc_center.x, + points->arc_center.y, + points->arc_center.z); + + //Verify tolerances + double h; + pmCartMag(&r_PC, &h); + tp_debug_print("center_dist = %f\n", h); + + double T_final = h - param->R_plan; + tp_debug_print("T_final = %f\n",T_final); + if (T_final > param->tolerance) { + tp_debug_print("Projected circle T (%f) exceeds tolerance %f, aborting blend arc\n", + T_final, + param->tolerance); + return TP_ERR_FAIL; + } + + points->trim1 = d_L; + + points->trim2 = findTrimAngle(&geom->P, + &points->arc_center, + &geom->center2); + + return TP_ERR_OK; +} + + +/** + * Take results of line blend calculation and project onto circular arc and line + */ +int blendArcLinePostProcess(BlendPoints3 * const points, + BlendPoints3 const * const points_in, + BlendParameters * const param, + BlendGeom3 const * const geom, + PmCircle const * const circ1, + PmCartLine const * const line2) +{ + (void)points_in; + (void)circ1; + (void)line2; + + // Define distance from actual arc center to circle center + double d1 = negate(param->R_plan, param->convex1) + geom->radius1; + tp_debug_print("d1 = %f\n", d1); + + //Get unit vector normal to line in plane, towards arc center + PmCartesian n2; + pmCartCartCross(&geom->binormal, &geom->u2, &n2); + pmCartUnitEq(&n2); + + tp_debug_print("n2 = %f %f %f\n", + n2.x, + n2.y, + n2.z); + + PmCartesian r_PC1; + pmCartCartSub(&geom->center1, &geom->P, &r_PC1); + double c1_u, c1_n; //Components of C1-P on u2 and n2 + pmCartCartDot(&r_PC1, &geom->u2, &c1_u); + pmCartCartDot(&r_PC1, &n2, &c1_n); + + double d_L; // badly named distance along line to intersection + double A = 1; + double B = 2.0 * c1_u; + double C = pmSq(c1_u) - pmSq(d1) + pmSq(param->R_plan - c1_n); + double root0,root1; + int res_dist = quadraticFormula(A, B, C, &root0, &root1); + if (res_dist) { + return TP_ERR_FAIL; + } + + tp_debug_print("root0 = %f, root1 = %f\n", root0, + root1); + d_L = fmin(fabs(root0),fabs(root1)); + if (d_L < 0) { + tp_debug_print("d_L can't be < 0, aborting...\n"); + return TP_ERR_FAIL; + } + + PmCartesian C_u, C_n; + + pmCartScalMult(&geom->u2, d_L, &C_u); + pmCartScalMult(&n2, param->R_plan, &C_n); + + PmCartesian r_PC; + //Continue with correct solution, get actual center + pmCartCartAdd(&C_u, &C_n, &r_PC); + pmCartCartAdd(&geom->P, &r_PC, &points->arc_center); + tp_debug_print("arc center = %f %f %f\n", + points->arc_center.x, + points->arc_center.y, + points->arc_center.z); + + //Verify tolerances + double h; + pmCartMag(&r_PC, &h); + tp_debug_print("center_dist = %f\n", h); + + double T_final = h - param->R_plan; + if (T_final > param->tolerance) { + tp_debug_print("Projected circle T (%f) exceeds tolerance %f, aborting blend arc\n", + T_final, + param->tolerance); + return TP_ERR_FAIL; + } + tp_debug_print("T_final = %f\n",T_final); + + points->trim1 = findTrimAngle(&geom->P, + &points->arc_center, + &geom->center1); + + points->trim2 = d_L; + + return TP_ERR_OK; +} + + +/** + * "Post-process" results from linear approximation to fit the circular segments. + * This step handles the projection from the linear approximation of each + * circle. Given the solved radius and tolerance, this function updates the + * points structure with the exact trim angles for each segment. + */ +int blendArcArcPostProcess(BlendPoints3 * const points, BlendPoints3 const * const points_in, + BlendParameters * const param, BlendGeom3 const * const geom, + PmCircle const * const circ1, PmCircle const * const circ2) +{ + (void)points_in; + (void)circ1; + (void)circ2; + + // Create "shifted center" approximation of spiral circles + // TODO refers to u1 instead of utan? + // Define distances from actual center to adjusted circle centers + double d1 = negate(param->R_plan, param->convex1) + geom->radius1; + double d2 = negate(param->R_plan, param->convex2) + geom->radius2; + tp_debug_print("d1 = %f, d2 = %f\n", d1, d2); + + //Find "x" distance between C1 and C2 + PmCartesian r_C1C2; + pmCartCartSub(&geom->center2, &geom->center1, &r_C1C2); + double c2x; + pmCartMag(&r_C1C2, &c2x); + + // Compute the new center location + + double Cx = (-pmSq(d1) + pmSq(d2)-pmSq(c2x)) / (-2.0 * c2x); + double Cy = pmSqrt(pmSq(d1) - pmSq(Cx)); + + tp_debug_print("Cx = %f, Cy = %f\n",Cx,Cy); + + // Find the basis vector uc from center1 to center2 + PmCartesian uc; + //TODO catch failures here + int norm_err = pmCartUnit(&r_C1C2, &uc); + if (norm_err) { + return TP_ERR_FAIL; + } + + // Find the basis vector perpendicular to the binormal and uc + PmCartesian nc; + pmCartCartCross(&geom->binormal, &uc, &nc); + + //Check if nc is in the same half-plane as the intersection normal. if not, + //we need to flip it around to choose the correct solution. + double dot1; + pmCartCartDot(&geom->normal, &nc, &dot1); + if (dot1 < 0) { + pmCartNegEq(&nc); + } + norm_err = pmCartUnitEq(&nc); + if (norm_err) { + return TP_ERR_FAIL; + } + + //Find components of center position wrt circle 1 center. + PmCartesian c_x, c_y; + pmCartScalMult(&uc, Cx, &c_x); + pmCartScalMult(&nc, Cy, &c_y); + + //Get vector from P to first center + PmCartesian r_PC1; + pmCartCartSub(&geom->center1, &geom->P, &r_PC1); + + // Get "test vectors, relative distance from solution center to P + PmCartesian test1, test2; + pmCartCartAdd(&r_PC1, &c_x, &test1); + test2=test1; + + //Add and subtract c_y component to get equivalent of two Y solutions + pmCartCartAddEq(&test1, &c_y); + pmCartCartSubEq(&test2, &c_y); + + double mag1,mag2; + pmCartMag(&test1, &mag1); + pmCartMag(&test2, &mag2); + + if (mag2 < mag1) + { + //negative solution is closer + pmCartNegEq(&c_y); + } + + //Continue with correct solution, get actual center + PmCartesian r_C1C; + pmCartCartAdd(&c_x, &c_y, &r_C1C); + pmCartCartAdd(&geom->center1, &r_C1C, &points->arc_center); + tp_debug_print("arc center = %f %f %f\n", + points->arc_center.x, + points->arc_center.y, + points->arc_center.z); + + //Find components of center position wrt circle 2 center. + PmCartesian r_C2C; + pmCartCartSub(&points->arc_center, &geom->center2, &r_C2C); + + PmCartesian r_PC; + pmCartCartSub(&points->arc_center, &geom->P, &r_PC); + + //Verify tolerances + double h; + pmCartMag(&r_PC, &h); + tp_debug_print("center_dist = %f\n", h); + + double T_final = h - param->R_plan; + if (T_final > param->tolerance) { + tp_debug_print("Projected circle T (%f) exceeds tolerance %f, aborting blend arc\n", + T_final, + param->tolerance); + return TP_ERR_FAIL; + } + tp_debug_print("T_final = %f\n",T_final); + + points->trim1 = findTrimAngle(&geom->P, + &points->arc_center, + &geom->center1); + points->trim2 = findTrimAngle(&geom->P, + &points->arc_center, + &geom->center2); + + tp_debug_print("trim1 = %f, trim2 = %f\n", + points->trim1, + points->trim2); + + return TP_ERR_OK; +} + + +/** + * Setup the spherical arc struct based on the blend arc data. + */ +int arcFromBlendPoints3(SphericalArc * const arc, BlendPoints3 const * const points, + BlendGeom3 const * const geom, BlendParameters const * const param) +{ + // If we consume the previous line, the remaining line length gets added here + arc->uTan = geom->u_tan1; + arc->line_length = param->line_length; + arc->binormal = geom->binormal; + + // Create the arc from the processed points + return arcInitFromPoints(arc, &points->arc_start, + &points->arc_end, &points->arc_center); +} + +int blendGeom3Print(BlendGeom3 const * const geom) +{ + (void)geom; + tp_debug_print("u1 = %f %f %f\n", + geom->u1.x, + geom->u1.y, + geom->u1.z); + + tp_debug_print("u2 = %f %f %f\n", + geom->u2.x, + geom->u2.y, + geom->u2.z); + return 0; +} + +int blendPoints3Print(BlendPoints3 const * const points) +{ + (void)points; + tp_debug_print("arc_start = %f %f %f\n", + points->arc_start.x, + points->arc_start.y, + points->arc_start.z); + + tp_debug_print("arc_center = %f %f %f\n", + points->arc_center.x, + points->arc_center.y, + points->arc_center.z); + + tp_debug_print("arc_end = %f %f %f\n", + points->arc_end.x, + points->arc_end.y, + points->arc_end.z); + + return 0; + +} + +double pmCartAbsMax(PmCartesian const * const v) +{ + return fmax(fmax(fabs(v->x),fabs(v->y)),fabs(v->z)); +} + + + + +/** @section spiralfuncs Functions to approximate spiral arc length */ + +/** + * Intermediate function to find the angle for a parameter from 0..1 along the + * spiral arc. + */ +static int pmCircleAngleFromParam(PmCircle const * const circle, + SpiralArcLengthFit const * const fit, + double t, + double * const angle) +{ + if (fit->spiral_in) { + t = 1.0 - t; + } + //TODO error or cleanup input to prevent param outside 0..1 + double s_in = t * fit->total_planar_length; + + // Quadratic formula to invert arc length -> angle + + double A = fit->b0; + double B = fit->b1; + double C = -s_in; + + double disc = pmSq(B) - 4.0 * A * C ; + if (disc < 0) { + rtapi_print_msg(RTAPI_MSG_ERR, "discriminant %f is negative in angle calculation\n",disc); + return TP_ERR_FAIL; + } + + /* + * Stability of inverting the arc-length relationship. + * Since the b1 coefficient is analogous to arc radius, we can be + * reasonably assured that it will be large enough not to cause numerical + * errors. If this is not the case, then the arc itself is degenerate (very + * small radius), and this condition should be caught well before here. + * + * Since an arc with a very small spiral coefficient will have a small b0 + * coefficient in the fit, we use the Citardauq Formula to ensure that the + * positive root does not lose precision due to subtracting near-similar values. + * + * For more information, see: + * http://people.csail.mit.edu/bkph/articles/Quadratics.pdf + */ + + double angle_out = (2.0 * C) / ( -B - pmSqrt(disc)); + + if (fit->spiral_in) { + // Spiral fit assumes that we're spiraling out, so + // parameterize from opposite end + angle_out = circle->angle - angle_out; + } + + *angle = angle_out; + return TP_ERR_OK; +} + + +static void printSpiralArcLengthFit(SpiralArcLengthFit const * const fit) +{ + (void)fit; + tp_debug_print("Spiral fit: b0 = %.12f, b1 = %.12f, length = %.12f, spiral_in = %d\n", + fit->b0, + fit->b1, + fit->total_planar_length, + fit->spiral_in); +} + +/** + * Approximate the arc length function of a general spiral. + * + * The closed-form arc length of a general archimedean spiral is rather + * computationally messy to work with. + * See http://mathworld.wolfram.com/ArchimedesSpiral.html for the actual form. + * + * The simplification here is made possible by a few assumptions: + * 1) That the spiral starts with a nonzero radius + * 2) The spiral coefficient (i.e. change in radius / angle) is not too large + * 3) The spiral coefficient has some minimum magnitude ("perfect" circles are handled as a special case) + * + * The 2nd-order fit below works by matching slope at the start and end of the + * arc length vs. angle curve. This completely specifies the 2nd order fit. + * Also, this fit predicts a total arc length >= the true arc length, which + * means the true speed along the curve will be the same or slower than the + * nominal speed. + */ +int findSpiralArcLengthFit(PmCircle const * const circle, + SpiralArcLengthFit * const fit) +{ + // Additional data for arc length approximation + double spiral_coef = circle->spiral / circle->angle; + double min_radius = circle->radius; + + if (fsign(circle->spiral) < 0.0) { + // Treat as positive spiral, parameterized in opposite + // direction + spiral_coef*=-1.0; + // Treat final radius as starting radius for fit, so we add the + // negative spiral term to get the minimum radius + // + min_radius+=circle->spiral; + fit->spiral_in = true; + } else { + fit->spiral_in = false; + } + tp_debug_print("radius = %.12f, angle = %.12f\n", min_radius, circle->angle); + tp_debug_print("spiral_coef = %.12f\n", spiral_coef); + + + //Compute the slope of the arc length vs. angle curve at the start and end of the segment + double slope_start = pmSqrt(pmSq(min_radius) + pmSq(spiral_coef)); + double slope_end = pmSqrt(pmSq(min_radius + spiral_coef * circle->angle) + pmSq(spiral_coef)); + + fit->b0 = (slope_end - slope_start) / (2.0 * circle->angle); + fit->b1 = slope_start; + + fit->total_planar_length = fit->b0 * pmSq(circle->angle) + fit->b1 * circle->angle; + printSpiralArcLengthFit(fit); + + // Check against start and end angle + double angle_end_chk = 0.0; + int res_angle = pmCircleAngleFromParam(circle, fit, 1.0, &angle_end_chk); + if (res_angle != TP_ERR_OK) { + //TODO better error message + rtapi_print_msg(RTAPI_MSG_ERR, + "Spiral fit failed\n"); + return TP_ERR_FAIL; + } + + // Check fit against angle + double fit_err = angle_end_chk - circle->angle; + if (fabs(fit_err) > TP_ANGLE_EPSILON) { + rtapi_print_msg(RTAPI_MSG_ERR, + "Spiral fit angle difference is %e, maximum allowed is %e\n", + fit_err, + TP_ANGLE_EPSILON); + return TP_ERR_FAIL; + } + + return TP_ERR_OK; +} + + +/** + * Compute the angle around a circular segment from the total progress along + * the curve. + */ +int pmCircleAngleFromProgress(PmCircle const * const circle, + SpiralArcLengthFit const * const fit, + double progress, + double * const angle) +{ + double h2; + pmCartMagSq(&circle->rHelix, &h2); + double s_end = pmSqrt(pmSq(fit->total_planar_length) + h2); + // Parameterize by total progress along helix + double t = progress / s_end; + return pmCircleAngleFromParam(circle, fit, t, angle); +} + + +/** + * Find the effective minimum radius for acceleration calculations. + * The radius of curvature of a spiral is larger than the circle of the same + * radius. + */ +double pmCircleEffectiveMinRadius(PmCircle const * circle) +{ + double dr = circle->spiral / circle->angle; + double h2; + pmCartMagSq(&circle->rHelix, &h2); + + // Exact representation of spiral arc length flattened into + double n_inner = pmSq(dr) + pmSq(circle->radius); + double den = n_inner+pmSq(dr); + double num = pmSqrt(n_inner * n_inner * n_inner); + double r_spiral = num / den; + + // Curvature of helix, assuming that helical motion is independent of plane motion + double effective_radius = h2 / r_spiral + r_spiral; + + return effective_radius; +} + diff --git a/wasm-port/vendor/linuxcnc/src/emc/tp/blendmath.h b/wasm-port/vendor/linuxcnc/src/emc/tp/blendmath.h new file mode 100644 index 0000000..f89efb7 --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/tp/blendmath.h @@ -0,0 +1,281 @@ +/******************************************************************** +* Description: blendmath.h +* Circular arc blend math functions +* +* Author: Robert W. Ellenberg +* License: GPL Version 2 +* System: Linux +* +* Copyright (c) 2014 All rights reserved. +* +* Last change: +********************************************************************/ +#ifndef BLENDMATH_H +#define BLENDMATH_H + +#include + +#include "tc_types.h" +#include "sp_scurve.h" + +#define BLEND_ACC_RATIO_TANGENTIAL 0.5 +#define BLEND_ACC_RATIO_NORMAL (pmSqrt(1.0 - pmSq(BLEND_ACC_RATIO_TANGENTIAL))) +#define BLEND_KINK_FACTOR 0.25 + +typedef enum { + BLEND_NONE, + BLEND_LINE_LINE, + BLEND_LINE_ARC, + BLEND_ARC_LINE, + BLEND_ARC_ARC, +} blend_type_t; + +/** + * 3D Input geometry for a spherical blend arc. + * This structure contains all of the basic geometry in 3D for a blend arc. + */ +typedef struct { + PmCartesian u1; /* unit vector along line 1 */ + PmCartesian u2; /* unit vector along line 2 */ + PmCartesian P; /* Intersection point */ + PmCartesian normal; /* normal unit vector to plane containing lines */ + PmCartesian binormal; /* binormal unit vector to plane containing lines */ + PmCartesian u_tan1; /* Actual tangent vector to 1 (used for arcs only) */ + PmCartesian u_tan2; /* Actual tangent vector to 2 (used for arcs only) */ + PmCartesian center1; /* Local approximation of center for arc 1 */ + PmCartesian center2; /* Local approximation of center for arc 2 */ + double radius1; /* Local approximation of radius */ + double radius2; + double theta_tan; + double v_max1; /* maximum velocity in direction u_tan1 */ + double v_max2; /* maximum velocity in direction u_tan2 */ + +} BlendGeom3; + +/** + * 9D Input geometry for a spherical blend arc. + */ +#ifdef BLEND_9D +typedef struct { +//Not implemented yet +} BlendGeom9; +#endif + + +/** + * Blend arc parameters (abstracted). + * This structure holds blend arc parameters that have been abstracted from the + * physical geometry. This data is used to find the maximum radius given the + * constraints on the blend. By abstracting the parameters from the geometry, + * the same calculations can be used with any input geometry (lines, arcs, 6 or + * 9 dimensional lines). + */ +typedef struct { + double tolerance; /* Net blend tolerance (min of line 1 and 2) */ + double L1; /* Available part of line 1 to blend over */ + double L2; /* Available part of line 2 to blend over */ + double v_req; /* requested velocity for the blend arc */ + double a_max; /* max acceleration allowed for blend */ + + /* These fields are considered "output", and may be refactored into a + * separate structure in the future */ + + double theta; /* Intersection angle, half of angle between -u1 and u2 */ + double phi; /* supplement of intersection angle, angle between u1 and u2 */ + double a_n_max; /* max normal acceleration allowed */ + + double R_plan; /* planned radius for blend arc */ + double d_plan; /* distance along each line to arc endpoints */ + + double v_goal; /* desired velocity at max feed override */ + double v_plan; /* planned max velocity at max feed override */ + double v_actual; /* velocity at feedscale = 1.0 */ + double s_arc; /* arc length */ + int consume; /* Consume the previous segment */ + double line_length; + //Arc specific stuff + int convex1; + int convex2; + double phi1_max; + double phi2_max; + +} BlendParameters; + + +/** + * Output geometry in 3D. + * Stores the three points representing a simple 3D spherical arc. + */ +typedef struct { + PmCartesian arc_start; /* start point for blend arc */ + PmCartesian arc_end; /* end point for blend arc */ + PmCartesian arc_center; /* center point for blend arc */ + double trim1; /* length (line) or angle (arc) to cut from prev_tc */ + double trim2; /* length (line) or angle (arc) to cut from tc */ +} BlendPoints3; + + + +#ifdef BLEND_9D +typedef struct { +//Not implemented yet +} BlendPoints9; +#endif + +double findMaxTangentAngle(double v, double acc, double cycle_time); + +double findKinkAccel(double kink_angle, double v_plan, double cycle_time); + +double fsign(double f); + +int clip_min(double * const x, double min); + +int clip_max(double * const x, double max); + +double saturate(double x, double max); + +double bisaturate(double x, double max, double min); + +int sat_inplace(double * const x, double max); + +int checkTangentAngle(PmCircle const * const circ, SphericalArc const * const arc, BlendGeom3 const * const geom, BlendParameters const * const param, double cycle_time, int at_end); + +int findIntersectionAngle(PmCartesian const * const u1, + PmCartesian const * const u2, double * const theta); + +double pmCartMin(PmCartesian const * const in); + +int calculateInscribedDiameter(PmCartesian const * const normal, + PmCartesian const * const bounds, double * const diameter); + +int findAccelScale(PmCartesian const * const acc, + PmCartesian const * const bounds, + PmCartesian * const scale); + +int pmUnitCartsColinear(PmCartesian const * const u1, + PmCartesian const * const u2); + +int pmCartCartParallel(PmCartesian const * const u1, + PmCartesian const * const u2, + double tol); + +int pmCartCartAntiParallel(PmCartesian const * const u1, + PmCartesian const * const u2, + double tol); + +int pmCircLineCoplanar(PmCircle const * const circ, + PmCartLine const * const line, double tol); + +int blendCoplanarCheck(PmCartesian const * const normal, + PmCartesian const * const u1_tan, + PmCartesian const * const u2_tan, + double tol); + +int blendCalculateNormals3(BlendGeom3 * const geom); + +int blendComputeParameters(BlendParameters * const param); + +int blendCheckConsume(BlendParameters * const param, + BlendPoints3 const * const points, + TC_STRUCT const * const prev_tc, int gap_cycles); + +int blendFindPoints3(BlendPoints3 * const points, BlendGeom3 const * const geom, + BlendParameters const * const param); + +int blendGeom3Init(BlendGeom3 * const geom, + TC_STRUCT const * const prev_tc, + TC_STRUCT const * const tc); + +int blendParamKinematics(BlendGeom3 * const geom, + BlendParameters * const param, + TC_STRUCT const * const prev_tc, + TC_STRUCT const * const tc, + PmCartesian const * const acc_bound, + PmCartesian const * const vel_bound, + double maxFeedScale); + +int blendInit3FromLineLine(BlendGeom3 * const geom, BlendParameters * const param, + TC_STRUCT const * const prev_tc, + TC_STRUCT const * const tc, + PmCartesian const * const acc_bound, + PmCartesian const * const vel_bound, + double maxFeedScale); + +int blendInit3FromLineArc(BlendGeom3 * const geom, BlendParameters * const param, + TC_STRUCT const * const prev_tc, + TC_STRUCT const * const tc, + PmCartesian const * const acc_bound, + PmCartesian const * const vel_bound, + double maxFeedScale); + +int blendInit3FromArcLine(BlendGeom3 * const geom, BlendParameters * const param, + TC_STRUCT const * const prev_tc, + TC_STRUCT const * const tc, + PmCartesian const * const acc_bound, + PmCartesian const * const vel_bound, + double maxFeedScale); + +int blendInit3FromArcArc(BlendGeom3 * const geom, BlendParameters * const param, + TC_STRUCT const * const prev_tc, + TC_STRUCT const * const tc, + PmCartesian const * const acc_bound, + PmCartesian const * const vel_bound, + double maxFeedScale); + +int blendArcArcPostProcess(BlendPoints3 * const points, BlendPoints3 const * const points_in, + BlendParameters * const param, BlendGeom3 const * const geom, + PmCircle const * const circ1, PmCircle const * const circ2); + +int blendLineArcPostProcess(BlendPoints3 * const points, BlendPoints3 const * const points_in, + BlendParameters * const param, BlendGeom3 const * const geom, + PmCartLine const * const line1, PmCircle const * const circ2); + +int blendArcLinePostProcess(BlendPoints3 * const points, BlendPoints3 const * const points_in, + BlendParameters * const param, BlendGeom3 const * const geom, + PmCircle const * const circ1, PmCartLine const * const line2); + +int arcFromBlendPoints3(SphericalArc * const arc, BlendPoints3 const * const points, + BlendGeom3 const * const geom, BlendParameters const * const param); + +//Not implemented yet +int blendGeom3Print(BlendGeom3 const * const geom); +int blendParamPrint(BlendParameters const * const param); +int blendPoints3Print(BlendPoints3 const * const points); + +double pmCartAbsMax(PmCartesian const * const v); + +int findSpiralArcLengthFit(PmCircle const * const circle, + SpiralArcLengthFit * const fit); +int pmCircleAngleFromProgress(PmCircle const * const circle, + SpiralArcLengthFit const * const fit, + double progress, + double * const angle); +double pmCircleEffectiveMinRadius(const PmCircle *circle); + +static inline double findVPeak(double a_t_max, double distance) +{ + return pmSqrt(a_t_max * distance); +} + + +static inline double findSCurveVPeak(double a_t_max, double j_t_max, double distance) +{ + // Parameter validation + if (a_t_max <= 0.0 || j_t_max <= 0.0 || distance <= 0.0) { + return 0.0; + } + + double triangular_v = findVPeak(a_t_max, distance); + + double req_v; + int result = findSCurveVSpeed(distance, a_t_max, j_t_max, &req_v); + + // If the S-curve calculation fails, revert to the simpler triangular velocity calculation. + if (result != 1) { + return triangular_v; + } + + // Take the smaller value between the S-curve velocity and the triangular velocity. + return fmin(req_v, triangular_v); +} +#endif diff --git a/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/block.c b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/block.c new file mode 100644 index 0000000..f49301e --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/block.c @@ -0,0 +1,138 @@ +/* + * cruckig - Pure C99 port of the Ruckig trajectory generation library + * Copyright (c) 2025 Yang Yang + * Copyright (c) 2021 Lars Berscheid (original C++ Ruckig) + * + * License: MIT, see the LICENSE file in this directory. + */ + +#include "block.h" + +static inline double cruckig_profile_total_duration(const CRuckigProfile *p) { + return p->t_sum[6] + p->brake.duration + p->accel.duration; +} + +static void remove_profile(CRuckigProfile *valid_profiles, size_t *valid_profile_counter, size_t index) { + for (size_t i = index; i < *valid_profile_counter - 1; ++i) { + valid_profiles[i] = valid_profiles[i + 1]; + } + *valid_profile_counter -= 1; +} + +static void interval_from_profiles(CRuckigInterval *iv, const CRuckigProfile *profile_left, const CRuckigProfile *profile_right) { + const double left_duration = cruckig_profile_total_duration(profile_left); + const double right_duration = cruckig_profile_total_duration(profile_right); + if (left_duration < right_duration) { + iv->left = left_duration; + iv->right = right_duration; + iv->profile = *profile_right; + } else { + iv->left = right_duration; + iv->right = left_duration; + iv->profile = *profile_left; + } + iv->valid = true; +} + +void cruckig_block_init(CRuckigBlock *block) { + cruckig_profile_init(&block->p_min); + block->t_min = 0.0; + block->a.valid = false; + block->b.valid = false; +} + +void cruckig_block_set_min_profile(CRuckigBlock *block, const CRuckigProfile *profile) { + block->p_min = *profile; + block->t_min = cruckig_profile_total_duration(profile); + block->a.valid = false; + block->b.valid = false; +} + +bool cruckig_block_calculate(CRuckigBlock *block, CRuckigProfile *valid_profiles, + size_t valid_profile_counter, size_t max_profiles) { + (void)max_profiles; + + if (valid_profile_counter == 1) { + cruckig_block_set_min_profile(block, &valid_profiles[0]); + return true; + + } else if (valid_profile_counter == 2) { + if (fabs(valid_profiles[0].t_sum[6] - valid_profiles[1].t_sum[6]) < 8 * DBL_EPSILON) { + cruckig_block_set_min_profile(block, &valid_profiles[0]); + return true; + } + + /* numerical_robust = true */ + { + const size_t idx_min = (valid_profiles[0].t_sum[6] < valid_profiles[1].t_sum[6]) ? 0 : 1; + const size_t idx_else_1 = (idx_min + 1) % 2; + + cruckig_block_set_min_profile(block, &valid_profiles[idx_min]); + interval_from_profiles(&block->a, &valid_profiles[idx_min], &valid_profiles[idx_else_1]); + return true; + } + + /* Only happens due to numerical issues */ + } else if (valid_profile_counter == 4) { + /* Find "identical" profiles */ + if (fabs(valid_profiles[0].t_sum[6] - valid_profiles[1].t_sum[6]) < 32 * DBL_EPSILON && valid_profiles[0].direction != valid_profiles[1].direction) { + remove_profile(valid_profiles, &valid_profile_counter, 1); + } else if (fabs(valid_profiles[2].t_sum[6] - valid_profiles[3].t_sum[6]) < 256 * DBL_EPSILON && valid_profiles[2].direction != valid_profiles[3].direction) { + remove_profile(valid_profiles, &valid_profile_counter, 3); + } else if (fabs(valid_profiles[0].t_sum[6] - valid_profiles[3].t_sum[6]) < 256 * DBL_EPSILON && valid_profiles[0].direction != valid_profiles[3].direction) { + remove_profile(valid_profiles, &valid_profile_counter, 3); + } else { + return false; + } + + } else if (valid_profile_counter % 2 == 0) { + return false; + } + + /* Find index of fastest profile */ + size_t idx_min = 0; + for (size_t i = 1; i < valid_profile_counter; ++i) { + if (valid_profiles[i].t_sum[6] < valid_profiles[idx_min].t_sum[6]) { + idx_min = i; + } + } + + cruckig_block_set_min_profile(block, &valid_profiles[idx_min]); + + if (valid_profile_counter == 3) { + const size_t idx_else_1 = (idx_min + 1) % 3; + const size_t idx_else_2 = (idx_min + 2) % 3; + + interval_from_profiles(&block->a, &valid_profiles[idx_else_1], &valid_profiles[idx_else_2]); + return true; + + } else if (valid_profile_counter == 5) { + const size_t idx_else_1 = (idx_min + 1) % 5; + const size_t idx_else_2 = (idx_min + 2) % 5; + const size_t idx_else_3 = (idx_min + 3) % 5; + const size_t idx_else_4 = (idx_min + 4) % 5; + + if (valid_profiles[idx_else_1].direction == valid_profiles[idx_else_2].direction) { + interval_from_profiles(&block->a, &valid_profiles[idx_else_1], &valid_profiles[idx_else_2]); + interval_from_profiles(&block->b, &valid_profiles[idx_else_3], &valid_profiles[idx_else_4]); + } else { + interval_from_profiles(&block->a, &valid_profiles[idx_else_1], &valid_profiles[idx_else_4]); + interval_from_profiles(&block->b, &valid_profiles[idx_else_2], &valid_profiles[idx_else_3]); + } + return true; + } + + return false; +} + +/* cruckig_block_is_blocked is now inlined in block.h */ + +const CRuckigProfile* cruckig_block_get_profile(const CRuckigBlock *block, double t) { + if (block->b.valid && t >= block->b.right) { + return &block->b.profile; + } + if (block->a.valid && t >= block->a.right) { + return &block->a.profile; + } + return &block->p_min; +} diff --git a/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/block.h b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/block.h new file mode 100644 index 0000000..5dfcd23 --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/block.h @@ -0,0 +1,43 @@ +/* + * cruckig - Pure C99 port of the Ruckig trajectory generation library + * Copyright (c) 2025 Yang Yang + * Copyright (c) 2021 Lars Berscheid (original C++ Ruckig) + * + * License: MIT, see the LICENSE file in this directory. + */ +#ifndef CRUCKIG_BLOCK_H +#define CRUCKIG_BLOCK_H + +#include "cruckig_internal.h" +#include "profile.h" + +typedef struct { + double left, right; + CRuckigProfile profile; + bool valid; +} CRuckigInterval; + +typedef struct { + CRuckigProfile p_min; + double t_min; + CRuckigInterval a; + CRuckigInterval b; +} CRuckigBlock; + +void cruckig_block_init(CRuckigBlock *block); +void cruckig_block_set_min_profile(CRuckigBlock *block, const CRuckigProfile *profile); + +/* Calculate block from valid profiles. Returns true if successful. */ +bool cruckig_block_calculate(CRuckigBlock *block, CRuckigProfile *valid_profiles, + size_t valid_profile_counter, size_t max_profiles); + +/* Inlined for hot-path performance (called in tight synchronization loop) */ +CRUCKIG_FORCE_INLINE bool cruckig_block_is_blocked(const CRuckigBlock *block, double t) { + return (t < block->t_min) + || (block->a.valid && block->a.left < t && t < block->a.right) + || (block->b.valid && block->b.left < t && t < block->b.right); +} + +const CRuckigProfile* cruckig_block_get_profile(const CRuckigBlock *block, double t); + +#endif /* CRUCKIG_BLOCK_H */ diff --git a/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/brake.c b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/brake.c new file mode 100644 index 0000000..7199134 --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/brake.c @@ -0,0 +1,201 @@ +/* + * cruckig - Pure C99 port of the Ruckig trajectory generation library + * Copyright (c) 2025 Yang Yang + * Copyright (c) 2021 Lars Berscheid (original C++ Ruckig) + * + * License: MIT, see the LICENSE file in this directory. + */ + +#include "brake.h" +#include "utils.h" + +static const double brake_eps = 2.2e-14; + +void cruckig_brake_init(CRuckigBrakeProfile *bp) { + bp->duration = 0.0; + bp->t[0] = 0.0; + bp->t[1] = 0.0; + bp->j[0] = 0.0; + bp->j[1] = 0.0; + bp->a[0] = 0.0; + bp->a[1] = 0.0; + bp->v[0] = 0.0; + bp->v[1] = 0.0; + bp->p[0] = 0.0; + bp->p[1] = 0.0; +} + +static inline double brake_v_at_t(double v0, double a0, double j, double t) { + return v0 + t * (a0 + j * t / 2); +} + +static inline double brake_v_at_a_zero(double v0, double a0, double j) { + return v0 + (a0 * a0) / (2 * j); +} + +static void acceleration_brake(CRuckigBrakeProfile *bp, double v0, double a0, + double vMax, double vMin, double aMax, double aMin, double jMax); +static void velocity_brake(CRuckigBrakeProfile *bp, double v0, double a0, + double vMax, double vMin, double aMax, double aMin, double jMax); + +static void acceleration_brake(CRuckigBrakeProfile *bp, double v0, double a0, + double vMax, double vMin, double aMax, double aMin, double jMax) { + bp->j[0] = -jMax; + + const double t_to_a_max = (a0 - aMax) / jMax; + const double t_to_a_zero = a0 / jMax; + + const double v_at_a_max = brake_v_at_t(v0, a0, -jMax, t_to_a_max); + const double v_at_a_zero_val = brake_v_at_t(v0, a0, -jMax, t_to_a_zero); + + if ((v_at_a_zero_val > vMax && jMax > 0) || (v_at_a_zero_val < vMax && jMax < 0)) { + velocity_brake(bp, v0, a0, vMax, vMin, aMax, aMin, jMax); + + } else if ((v_at_a_max < vMin && jMax > 0) || (v_at_a_max > vMin && jMax < 0)) { + const double t_to_v_min = -(v_at_a_max - vMin) / aMax; + const double t_to_v_max = -aMax / (2 * jMax) - (v_at_a_max - vMax) / aMax; + + bp->t[0] = t_to_a_max + brake_eps; + { + double val = t_to_v_min < (t_to_v_max - brake_eps) ? t_to_v_min : (t_to_v_max - brake_eps); + bp->t[1] = val > 0.0 ? val : 0.0; + } + + } else { + bp->t[0] = t_to_a_max + brake_eps; + } +} + +static void velocity_brake(CRuckigBrakeProfile *bp, double v0, double a0, + double vMax, double vMin, double aMax, double aMin, double jMax) { + (void)aMax; + bp->j[0] = -jMax; + const double t_to_a_min = (a0 - aMin) / jMax; + const double t_to_v_max = a0 / jMax + sqrt(a0 * a0 + 2 * jMax * (v0 - vMax)) / fabs(jMax); + const double t_to_v_min = a0 / jMax + sqrt(a0 * a0 / 2 + jMax * (v0 - vMin)) / fabs(jMax); + const double t_min_to_v_max = t_to_v_max < t_to_v_min ? t_to_v_max : t_to_v_min; + + if (t_to_a_min < t_min_to_v_max) { + const double v_at_a_min = brake_v_at_t(v0, a0, -jMax, t_to_a_min); + const double t_to_v_max_with_constant = -(v_at_a_min - vMax) / aMin; + const double t_to_v_min_with_constant = aMin / (2 * jMax) - (v_at_a_min - vMin) / aMin; + + bp->t[0] = (t_to_a_min - brake_eps) > 0.0 ? (t_to_a_min - brake_eps) : 0.0; + { + double val = t_to_v_max_with_constant < t_to_v_min_with_constant ? t_to_v_max_with_constant : t_to_v_min_with_constant; + bp->t[1] = val > 0.0 ? val : 0.0; + } + + } else { + bp->t[0] = (t_min_to_v_max - brake_eps) > 0.0 ? (t_min_to_v_max - brake_eps) : 0.0; + } +} + +void cruckig_brake_get_position_brake_trajectory(CRuckigBrakeProfile *bp, double v0, double a0, + double vMax, double vMin, double aMax, double aMin, double jMax) { + bp->t[0] = 0.0; + bp->t[1] = 0.0; + bp->j[0] = 0.0; + bp->j[1] = 0.0; + + if (jMax == 0.0 || aMax == 0.0 || aMin == 0.0) { + return; /* Ignore braking for zero-limits */ + } + + if (a0 > aMax) { + acceleration_brake(bp, v0, a0, vMax, vMin, aMax, aMin, jMax); + + } else if (a0 < aMin) { + acceleration_brake(bp, v0, a0, vMin, vMax, aMin, aMax, -jMax); + + } else if ((v0 > vMax && brake_v_at_a_zero(v0, a0, -jMax) > vMin) || (a0 > 0 && brake_v_at_a_zero(v0, a0, jMax) > vMax)) { + velocity_brake(bp, v0, a0, vMax, vMin, aMax, aMin, jMax); + + } else if ((v0 < vMin && brake_v_at_a_zero(v0, a0, jMax) < vMax) || (a0 < 0 && brake_v_at_a_zero(v0, a0, -jMax) < vMin)) { + velocity_brake(bp, v0, a0, vMin, vMax, aMin, aMax, -jMax); + } +} + +void cruckig_brake_get_second_order_position_brake_trajectory(CRuckigBrakeProfile *bp, double v0, + double vMax, double vMin, double aMax, double aMin) { + bp->t[0] = 0.0; + bp->t[1] = 0.0; + bp->j[0] = 0.0; + bp->j[1] = 0.0; + bp->a[0] = 0.0; + bp->a[1] = 0.0; + + if (aMax == 0.0 || aMin == 0.0) { + return; /* Ignore braking for zero-limits */ + } + + if (v0 > vMax) { + bp->a[0] = aMin; + bp->t[0] = (vMax - v0) / aMin + brake_eps; + + } else if (v0 < vMin) { + bp->a[0] = aMax; + bp->t[0] = (vMin - v0) / aMax + brake_eps; + } +} + +void cruckig_brake_get_velocity_brake_trajectory(CRuckigBrakeProfile *bp, double a0, + double aMax, double aMin, double jMax) { + bp->t[0] = 0.0; + bp->t[1] = 0.0; + bp->j[0] = 0.0; + bp->j[1] = 0.0; + + if (jMax == 0.0) { + return; /* Ignore braking for zero-limits */ + } + + if (a0 > aMax) { + bp->j[0] = -jMax; + bp->t[0] = (a0 - aMax) / jMax + brake_eps; + + } else if (a0 < aMin) { + bp->j[0] = jMax; + bp->t[0] = -(a0 - aMin) / jMax + brake_eps; + } +} + +void cruckig_brake_get_second_order_velocity_brake_trajectory(CRuckigBrakeProfile *bp) { + bp->t[0] = 0.0; + bp->t[1] = 0.0; + bp->j[0] = 0.0; + bp->j[1] = 0.0; +} + +void cruckig_brake_finalize(CRuckigBrakeProfile *bp, double *ps, double *vs, double *as) { + if (bp->t[0] <= 0.0 && bp->t[1] <= 0.0) { + bp->duration = 0.0; + return; + } + + bp->duration = bp->t[0]; + bp->p[0] = *ps; + bp->v[0] = *vs; + bp->a[0] = *as; + cruckig_integrate(bp->t[0], *ps, *vs, *as, bp->j[0], ps, vs, as); + + if (bp->t[1] > 0.0) { + bp->duration += bp->t[1]; + bp->p[1] = *ps; + bp->v[1] = *vs; + bp->a[1] = *as; + cruckig_integrate(bp->t[1], *ps, *vs, *as, bp->j[1], ps, vs, as); + } +} + +void cruckig_brake_finalize_second_order(CRuckigBrakeProfile *bp, double *ps, double *vs, double *as) { + if (bp->t[0] <= 0.0) { + bp->duration = 0.0; + return; + } + + bp->duration = bp->t[0]; + bp->p[0] = *ps; + bp->v[0] = *vs; + cruckig_integrate(bp->t[0], *ps, *vs, bp->a[0], 0.0, ps, vs, as); +} diff --git a/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/brake.h b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/brake.h new file mode 100644 index 0000000..4edb508 --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/brake.h @@ -0,0 +1,38 @@ +/* + * cruckig - Pure C99 port of the Ruckig trajectory generation library + * Copyright (c) 2025 Yang Yang + * Copyright (c) 2021 Lars Berscheid (original C++ Ruckig) + * + * License: MIT, see the LICENSE file in this directory. + */ +#ifndef CRUCKIG_BRAKE_H +#define CRUCKIG_BRAKE_H + +#include "cruckig_internal.h" + +/* Two-phase brake profile */ +typedef struct { + double duration; + double t[2]; + double j[2]; + double a[2]; + double v[2]; + double p[2]; +} CRuckigBrakeProfile; + +void cruckig_brake_init(CRuckigBrakeProfile *bp); + +/* Calculate brake trajectories */ +void cruckig_brake_get_position_brake_trajectory(CRuckigBrakeProfile *bp, double v0, double a0, + double vMax, double vMin, double aMax, double aMin, double jMax); +void cruckig_brake_get_second_order_position_brake_trajectory(CRuckigBrakeProfile *bp, double v0, + double vMax, double vMin, double aMax, double aMin); +void cruckig_brake_get_velocity_brake_trajectory(CRuckigBrakeProfile *bp, double a0, + double aMax, double aMin, double jMax); +void cruckig_brake_get_second_order_velocity_brake_trajectory(CRuckigBrakeProfile *bp); + +/* Finalize by integrating */ +void cruckig_brake_finalize(CRuckigBrakeProfile *bp, double *ps, double *vs, double *as); +void cruckig_brake_finalize_second_order(CRuckigBrakeProfile *bp, double *ps, double *vs, double *as); + +#endif /* CRUCKIG_BRAKE_H */ diff --git a/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/calculator.c b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/calculator.c new file mode 100644 index 0000000..4f36f77 --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/calculator.c @@ -0,0 +1,950 @@ +/* + * cruckig - Pure C99 port of the Ruckig trajectory generation library + * Copyright (c) 2025 Yang Yang + * Copyright (c) 2021 Lars Berscheid (original C++ Ruckig) + * + * License: MIT, see the LICENSE file in this directory. + */ +#include "calculator.h" +#include "position.h" +#include "velocity.h" +#include "utils.h" + + +static const double eps = DBL_EPSILON; + +CRuckigCalculator* cruckig_calculator_create(size_t dofs) { + CRuckigCalculator *calc = (CRuckigCalculator*)cruckig_calloc(1, sizeof(CRuckigCalculator)); + if (!calc) return NULL; + + calc->degrees_of_freedom = dofs; + + calc->new_phase_control = (double*)cruckig_calloc(dofs, sizeof(double)); + calc->pd = (double*)cruckig_calloc(dofs, sizeof(double)); + calc->possible_t_syncs = (double*)cruckig_calloc(3 * dofs + 1, sizeof(double)); + calc->idx = (size_t*)cruckig_calloc(3 * dofs + 1, sizeof(size_t)); + calc->blocks = (CRuckigBlock*)cruckig_calloc(dofs, sizeof(CRuckigBlock)); + calc->inp_min_velocity = (double*)cruckig_calloc(dofs, sizeof(double)); + calc->inp_min_acceleration = (double*)cruckig_calloc(dofs, sizeof(double)); + calc->inp_per_dof_control_interface = (CRuckigControlInterface*)cruckig_calloc(dofs, sizeof(CRuckigControlInterface)); + calc->inp_per_dof_synchronization = (CRuckigSynchronization*)cruckig_calloc(dofs, sizeof(CRuckigSynchronization)); + calc->segment_input = NULL; /* Created on demand for waypoint calculation */ + + if (!calc->new_phase_control || !calc->pd || !calc->possible_t_syncs || + !calc->idx || !calc->blocks || !calc->inp_min_velocity || + !calc->inp_min_acceleration || !calc->inp_per_dof_control_interface || + !calc->inp_per_dof_synchronization) { + cruckig_calculator_destroy(calc); + return NULL; + } + + for (size_t i = 0; i < dofs; ++i) { + cruckig_block_init(&calc->blocks[i]); + } + + return calc; +} + +void cruckig_calculator_destroy(CRuckigCalculator *calc) { + if (!calc) return; + cruckig_free(calc->new_phase_control); + cruckig_free(calc->pd); + cruckig_free(calc->possible_t_syncs); + cruckig_free(calc->idx); + cruckig_free(calc->blocks); + cruckig_free(calc->inp_min_velocity); + cruckig_free(calc->inp_min_acceleration); + cruckig_free(calc->inp_per_dof_control_interface); + cruckig_free(calc->inp_per_dof_synchronization); + cruckig_input_destroy(calc->segment_input); + cruckig_free(calc); +} + +/* Is the trajectory (in principle) phase synchronizable? */ +static bool is_input_collinear(CRuckigCalculator *calc, + const CRuckigInputParameter *inp, + CRuckigDirection limiting_direction, + size_t limiting_dof) +{ + const size_t dofs = calc->degrees_of_freedom; + + /* Compute pd = target_position - current_position */ + for (size_t dof = 0; dof < dofs; ++dof) { + calc->pd[dof] = inp->target_position[dof] - inp->current_position[dof]; + } + + /* Find scale vector and scale DOF */ + const double *scale_vector = NULL; + size_t scale_dof = 0; + bool scale_dof_found = false; + + for (size_t dof = 0; dof < dofs; ++dof) { + if (calc->inp_per_dof_synchronization[dof] != CRuckigSyncPhase) { + continue; + } + + if (calc->inp_per_dof_control_interface[dof] == CRuckigPosition && fabs(calc->pd[dof]) > eps) { + scale_vector = calc->pd; + scale_dof = dof; + scale_dof_found = true; + break; + } else if (fabs(inp->current_velocity[dof]) > eps) { + scale_vector = inp->current_velocity; + scale_dof = dof; + scale_dof_found = true; + break; + } else if (fabs(inp->current_acceleration[dof]) > eps) { + scale_vector = inp->current_acceleration; + scale_dof = dof; + scale_dof_found = true; + break; + } else if (fabs(inp->target_velocity[dof]) > eps) { + scale_vector = inp->target_velocity; + scale_dof = dof; + scale_dof_found = true; + break; + } else if (fabs(inp->target_acceleration[dof]) > eps) { + scale_vector = inp->target_acceleration; + scale_dof = dof; + scale_dof_found = true; + break; + } + } + + if (!scale_dof_found) { + return false; + } + + const double scale = scale_vector[scale_dof]; + const double pd_scale = calc->pd[scale_dof] / scale; + const double v0_scale = inp->current_velocity[scale_dof] / scale; + const double vf_scale = inp->target_velocity[scale_dof] / scale; + const double a0_scale = inp->current_acceleration[scale_dof] / scale; + const double af_scale = inp->target_acceleration[scale_dof] / scale; + + const double scale_limiting = scale_vector[limiting_dof]; + double control_limiting; + if (isinf(inp->max_jerk[limiting_dof])) { + control_limiting = (limiting_direction == DirectionUP) + ? inp->max_acceleration[limiting_dof] + : calc->inp_min_acceleration[limiting_dof]; + } else { + control_limiting = (limiting_direction == DirectionUP) + ? inp->max_jerk[limiting_dof] + : -inp->max_jerk[limiting_dof]; + } + + for (size_t dof = 0; dof < dofs; ++dof) { + if (calc->inp_per_dof_synchronization[dof] != CRuckigSyncPhase) { + continue; + } + + const double current_scale = scale_vector[dof]; + if ( + (calc->inp_per_dof_control_interface[dof] == CRuckigPosition && fabs(calc->pd[dof] - pd_scale * current_scale) > eps) + || fabs(inp->current_velocity[dof] - v0_scale * current_scale) > eps + || fabs(inp->current_acceleration[dof] - a0_scale * current_scale) > eps + || fabs(inp->target_velocity[dof] - vf_scale * current_scale) > eps + || fabs(inp->target_acceleration[dof] - af_scale * current_scale) > eps + ) { + return false; + } + + calc->new_phase_control[dof] = control_limiting * current_scale / scale_limiting; + } + + return true; +} + +/* Simple insertion sort for index array by values */ +static void sort_indices(size_t *idx_arr, const double *values, size_t count) { + for (size_t i = 1; i < count; ++i) { + size_t key = idx_arr[i]; + double key_val = values[key]; + size_t j = i; + while (j > 0 && values[idx_arr[j - 1]] > key_val) { + idx_arr[j] = idx_arr[j - 1]; + --j; + } + idx_arr[j] = key; + } +} + +/* + * synchronize: Find a valid synchronization time. + * Returns true if found; sets t_sync, limiting_dof, and updates profiles. + * + * has_t_min: whether t_min is valid + * t_min: minimum duration + * limiting_dof_out: set to the limiting DOF index; has_limiting_dof set to true/false + */ +static bool synchronize(CRuckigCalculator *calc, + bool has_t_min, double t_min, + double *t_sync, + bool *has_limiting_dof, size_t *limiting_dof_out, + CRuckigProfile *profiles, + bool discrete_duration, double delta_time) +{ + const size_t dofs = calc->degrees_of_freedom; + + /* Fill possible_t_syncs */ + bool any_interval = false; + for (size_t dof = 0; dof < dofs; ++dof) { + if (calc->inp_per_dof_synchronization[dof] == CRuckigSyncNone) { + calc->possible_t_syncs[dof] = 0.0; + calc->possible_t_syncs[dofs + dof] = INFINITY; + calc->possible_t_syncs[2 * dofs + dof] = INFINITY; + continue; + } + + calc->possible_t_syncs[dof] = calc->blocks[dof].t_min; + calc->possible_t_syncs[dofs + dof] = calc->blocks[dof].a.valid + ? calc->blocks[dof].a.right : INFINITY; + calc->possible_t_syncs[2 * dofs + dof] = calc->blocks[dof].b.valid + ? calc->blocks[dof].b.right : INFINITY; + any_interval = any_interval || calc->blocks[dof].a.valid || calc->blocks[dof].b.valid; + } + calc->possible_t_syncs[3 * dofs] = has_t_min ? t_min : INFINITY; + any_interval = any_interval || has_t_min; + + /* Discrete duration rounding */ + if (discrete_duration) { + size_t count = 3 * dofs + 1; + for (size_t i = 0; i < count; ++i) { + if (isinf(calc->possible_t_syncs[i])) continue; + double remainder = fmod(calc->possible_t_syncs[i], delta_time); + if (remainder > eps) { + calc->possible_t_syncs[i] += delta_time - remainder; + } + } + } + + /* Initialize and sort indices */ + size_t idx_end_count = any_interval ? (3 * dofs + 1) : dofs; + for (size_t i = 0; i < idx_end_count; ++i) { + calc->idx[i] = i; + } + sort_indices(calc->idx, calc->possible_t_syncs, idx_end_count); + + /* Start at dofs-1 (skip the dofs-1 smallest t_min values since we need ALL dofs at or past their t_min) */ + size_t start_idx = (dofs >= 1) ? (dofs - 1) : 0; + for (size_t iter = start_idx; iter < idx_end_count; ++iter) { + size_t i = calc->idx[iter]; + double possible_t_sync = calc->possible_t_syncs[i]; + + /* Check if any DOF is blocked */ + bool is_blocked = false; + for (size_t dof = 0; dof < dofs; ++dof) { + if (calc->inp_per_dof_synchronization[dof] == CRuckigSyncNone) { + continue; + } + if (cruckig_block_is_blocked(&calc->blocks[dof], possible_t_sync)) { + is_blocked = true; + break; + } + } + + double t_min_or_zero = has_t_min ? t_min : 0.0; + if (is_blocked || possible_t_sync < t_min_or_zero || isinf(possible_t_sync)) { + continue; + } + + *t_sync = possible_t_sync; + + if (i == 3 * dofs) { + /* Optional t_min was the winning candidate */ + *has_limiting_dof = false; + return true; + } + + /* Determine which DOF and which block part */ + size_t quot = i / dofs; + size_t rem = i % dofs; + *limiting_dof_out = rem; + *has_limiting_dof = true; + + switch (quot) { + case 0: + profiles[rem] = calc->blocks[rem].p_min; + break; + case 1: + profiles[rem] = calc->blocks[rem].a.profile; + break; + case 2: + profiles[rem] = calc->blocks[rem].b.profile; + break; + } + return true; + } + + return false; +} + +CRUCKIG_HOT +/* + * Find the optimal profile for a single DOF (Step 1). + * Separated to keep large Step1 structs (~3.6KB) off the main function's stack, + * which matters for the kernel's limited stack size. + */ +static bool find_profile_step1( + CRuckigCalculator *calc, + const CRuckigInputParameter *inp, + CRuckigProfile *p, + size_t dof) +{ + switch (calc->inp_per_dof_control_interface[dof]) { + case CRuckigPosition: { + if (!isinf(inp->max_jerk[dof])) { + CRuckigPositionThirdOrderStep1 *step1 = &calc->step1_workspace.pos3_step1; + cruckig_pos3_step1_init(step1, + p->p[0], p->v[0], p->a[0], p->pf, p->vf, p->af, + inp->max_velocity[dof], calc->inp_min_velocity[dof], + inp->max_acceleration[dof], calc->inp_min_acceleration[dof], + inp->max_jerk[dof]); + return cruckig_pos3_step1_get_profile(step1, p, &calc->blocks[dof]); + } else if (!isinf(inp->max_acceleration[dof])) { + CRuckigPositionSecondOrderStep1 *step1 = &calc->step1_workspace.pos2_step1; + cruckig_pos2_step1_init(step1, + p->p[0], p->v[0], p->pf, p->vf, + inp->max_velocity[dof], calc->inp_min_velocity[dof], + inp->max_acceleration[dof], calc->inp_min_acceleration[dof]); + return cruckig_pos2_step1_get_profile(step1, p, &calc->blocks[dof]); + } else { + CRuckigPositionFirstOrderStep1 *step1 = &calc->step1_workspace.pos1_step1; + cruckig_pos1_step1_init(step1, + p->p[0], p->pf, + inp->max_velocity[dof], calc->inp_min_velocity[dof]); + return cruckig_pos1_step1_get_profile(step1, p, &calc->blocks[dof]); + } + } break; + case CRuckigVelocity: { + if (!isinf(inp->max_jerk[dof])) { + CRuckigVelocityThirdOrderStep1 *step1 = &calc->step1_workspace.vel3_step1; + cruckig_vel3_step1_init(step1, + p->v[0], p->a[0], p->vf, p->af, + inp->max_acceleration[dof], calc->inp_min_acceleration[dof], + inp->max_jerk[dof]); + return cruckig_vel3_step1_get_profile(step1, p, &calc->blocks[dof]); + } else { + CRuckigVelocitySecondOrderStep1 *step1 = &calc->step1_workspace.vel2_step1; + cruckig_vel2_step1_init(step1, + p->v[0], p->vf, + inp->max_acceleration[dof], calc->inp_min_acceleration[dof]); + return cruckig_vel2_step1_get_profile(step1, p, &calc->blocks[dof]); + } + } break; + } + return false; +} + +CRuckigResult cruckig_calculator_calculate(CRuckigCalculator *calc, + const CRuckigInputParameter *inp, + CRuckigTrajectory *traj, + double delta_time, + bool *was_interrupted) +{ + *was_interrupted = false; + const size_t dofs = calc->degrees_of_freedom; + + for (size_t dof = 0; dof < dofs; ++dof) { + CRuckigProfile *p = &traj->profiles[dof]; + + calc->inp_min_velocity[dof] = inp->min_velocity + ? inp->min_velocity[dof] : -inp->max_velocity[dof]; + calc->inp_min_acceleration[dof] = inp->min_acceleration + ? inp->min_acceleration[dof] : -inp->max_acceleration[dof]; + calc->inp_per_dof_control_interface[dof] = inp->per_dof_control_interface + ? inp->per_dof_control_interface[dof] : inp->control_interface; + calc->inp_per_dof_synchronization[dof] = inp->per_dof_synchronization + ? inp->per_dof_synchronization[dof] : inp->synchronization; + + if (!inp->enabled[dof]) { + p->p[7] = inp->current_position[dof]; + p->v[7] = inp->current_velocity[dof]; + p->a[7] = inp->current_acceleration[dof]; + p->t_sum[6] = 0.0; + calc->blocks[dof].t_min = 0.0; + calc->blocks[dof].a.valid = false; + calc->blocks[dof].b.valid = false; + continue; + } + + /* Calculate brake (if input exceeds or will exceed limits) */ + switch (calc->inp_per_dof_control_interface[dof]) { + case CRuckigPosition: { + if (!isinf(inp->max_jerk[dof])) { + cruckig_brake_get_position_brake_trajectory(&p->brake, + inp->current_velocity[dof], inp->current_acceleration[dof], + inp->max_velocity[dof], calc->inp_min_velocity[dof], + inp->max_acceleration[dof], calc->inp_min_acceleration[dof], + inp->max_jerk[dof]); + } else if (!isinf(inp->max_acceleration[dof])) { + cruckig_brake_get_second_order_position_brake_trajectory(&p->brake, + inp->current_velocity[dof], + inp->max_velocity[dof], calc->inp_min_velocity[dof], + inp->max_acceleration[dof], calc->inp_min_acceleration[dof]); + } + cruckig_profile_set_boundary(p, + inp->current_position[dof], inp->current_velocity[dof], + inp->current_acceleration[dof], + inp->target_position[dof], inp->target_velocity[dof], + inp->target_acceleration[dof]); + } break; + case CRuckigVelocity: { + if (!isinf(inp->max_jerk[dof])) { + cruckig_brake_get_velocity_brake_trajectory(&p->brake, + inp->current_acceleration[dof], + inp->max_acceleration[dof], calc->inp_min_acceleration[dof], + inp->max_jerk[dof]); + } else { + cruckig_brake_get_second_order_velocity_brake_trajectory(&p->brake); + } + cruckig_profile_set_boundary_for_velocity(p, + inp->current_position[dof], inp->current_velocity[dof], + inp->current_acceleration[dof], + inp->target_velocity[dof], inp->target_acceleration[dof]); + } break; + } + + /* Finalize pre-trajectory */ + if (!isinf(inp->max_jerk[dof])) { + cruckig_brake_finalize(&p->brake, &p->p[0], &p->v[0], &p->a[0]); + } else if (!isinf(inp->max_acceleration[dof])) { + cruckig_brake_finalize_second_order(&p->brake, &p->p[0], &p->v[0], &p->a[0]); + } + + if (!find_profile_step1(calc, inp, p, dof)) { + bool has_zero_limits = (inp->max_acceleration[dof] == 0.0 || + calc->inp_min_acceleration[dof] == 0.0 || + inp->max_jerk[dof] == 0.0); + if (has_zero_limits) { + return CRuckigErrorZeroLimits; + } else { + return CRuckigErrorExecutionTimeCalculation; + } + } + + traj->independent_min_durations[dof] = calc->blocks[dof].t_min; + } + + const bool discrete_duration = (inp->duration_discretization == CRuckigDiscrete); + + if (dofs == 1 && !inp->has_minimum_duration && !discrete_duration) { + traj->duration = calc->blocks[0].t_min; + traj->profiles[0] = calc->blocks[0].p_min; + traj->cumulative_times[0] = traj->duration; + return CRuckigWorking; + } + + /* Synchronize */ + bool has_limiting_dof = false; + size_t limiting_dof = 0; + bool found_synchronization = synchronize(calc, + inp->has_minimum_duration, inp->minimum_duration, + &traj->duration, &has_limiting_dof, &limiting_dof, + traj->profiles, discrete_duration, delta_time); + + if (!found_synchronization) { + bool has_zero_limits = false; + for (size_t dof = 0; dof < dofs; ++dof) { + if (inp->max_acceleration[dof] == 0.0 || + calc->inp_min_acceleration[dof] == 0.0 || + inp->max_jerk[dof] == 0.0) { + has_zero_limits = true; + break; + } + } + if (has_zero_limits) { + return CRuckigErrorZeroLimits; + } else { + return CRuckigErrorSynchronizationCalculation; + } + } + + /* None Synchronization */ + for (size_t dof = 0; dof < dofs; ++dof) { + if (inp->enabled[dof] && calc->inp_per_dof_synchronization[dof] == CRuckigSyncNone) { + traj->profiles[dof] = calc->blocks[dof].p_min; + if (calc->blocks[dof].t_min > traj->duration) { + traj->duration = calc->blocks[dof].t_min; + has_limiting_dof = true; + limiting_dof = dof; + } + } + } + traj->cumulative_times[0] = traj->duration; + + /* Check maximal duration */ + if (traj->duration > 7.6e3) { + return CRuckigErrorTrajectoryDuration; + } + + if (traj->duration == 0.0) { + /* Copy all profiles for end state */ + for (size_t dof = 0; dof < dofs; ++dof) { + traj->profiles[dof] = calc->blocks[dof].p_min; + } + return CRuckigWorking; + } + + /* Check if all synchronizations are None */ + if (!discrete_duration) { + bool all_none = true; + for (size_t dof = 0; dof < dofs; ++dof) { + if (calc->inp_per_dof_synchronization[dof] != CRuckigSyncNone) { + all_none = false; + break; + } + } + if (all_none) { + return CRuckigWorking; + } + } + + /* Phase Synchronization */ + if (has_limiting_dof) { + bool any_phase = false; + for (size_t dof = 0; dof < dofs; ++dof) { + if (calc->inp_per_dof_synchronization[dof] == CRuckigSyncPhase) { + any_phase = true; + break; + } + } + + if (any_phase) { + const CRuckigProfile *p_limiting = &traj->profiles[limiting_dof]; + if (is_input_collinear(calc, inp, p_limiting->direction, limiting_dof)) { + bool found_time_synchronization = true; + + for (size_t dof = 0; dof < dofs; ++dof) { + if (!inp->enabled[dof] || dof == limiting_dof || + calc->inp_per_dof_synchronization[dof] != CRuckigSyncPhase) { + continue; + } + + CRuckigProfile *p = &traj->profiles[dof]; + double t_profile = traj->duration - p->brake.duration - p->accel.duration; + + /* Copy timing information from limiting DOF */ + memcpy(p->t, p_limiting->t, sizeof(p->t)); + p->control_signs = p_limiting->control_signs; + + switch (calc->inp_per_dof_control_interface[dof]) { + case CRuckigPosition: { + switch (p->control_signs) { + case ControlSignsUDDU: { + if (!isinf(inp->max_jerk[dof])) { + found_time_synchronization &= cruckig_profile_check_with_timing_full(p, + ControlSignsUDDU, ReachedLimitsNONE, + t_profile, calc->new_phase_control[dof], + inp->max_velocity[dof], calc->inp_min_velocity[dof], + inp->max_acceleration[dof], calc->inp_min_acceleration[dof], + inp->max_jerk[dof]); + } else if (!isinf(inp->max_acceleration[dof])) { + found_time_synchronization &= cruckig_profile_check_for_second_order_with_timing_full(p, + ControlSignsUDDU, ReachedLimitsNONE, + t_profile, calc->new_phase_control[dof], + -calc->new_phase_control[dof], + inp->max_velocity[dof], calc->inp_min_velocity[dof], + inp->max_acceleration[dof], calc->inp_min_acceleration[dof]); + } else { + found_time_synchronization &= cruckig_profile_check_for_first_order_with_timing_full(p, + ControlSignsUDDU, ReachedLimitsNONE, + t_profile, calc->new_phase_control[dof], + inp->max_velocity[dof], calc->inp_min_velocity[dof]); + } + } break; + case ControlSignsUDUD: { + if (!isinf(inp->max_jerk[dof])) { + found_time_synchronization &= cruckig_profile_check_with_timing_full(p, + ControlSignsUDUD, ReachedLimitsNONE, + t_profile, calc->new_phase_control[dof], + inp->max_velocity[dof], calc->inp_min_velocity[dof], + inp->max_acceleration[dof], calc->inp_min_acceleration[dof], + inp->max_jerk[dof]); + } else { + found_time_synchronization &= cruckig_profile_check_for_second_order_with_timing_full(p, + ControlSignsUDUD, ReachedLimitsNONE, + t_profile, calc->new_phase_control[dof], + -calc->new_phase_control[dof], + inp->max_velocity[dof], calc->inp_min_velocity[dof], + inp->max_acceleration[dof], calc->inp_min_acceleration[dof]); + } + } break; + } + } break; + case CRuckigVelocity: { + switch (p->control_signs) { + case ControlSignsUDDU: { + if (!isinf(inp->max_jerk[dof])) { + found_time_synchronization &= cruckig_profile_check_for_velocity_with_timing_full(p, + ControlSignsUDDU, ReachedLimitsNONE, + t_profile, calc->new_phase_control[dof], + inp->max_acceleration[dof], calc->inp_min_acceleration[dof], + inp->max_jerk[dof]); + } else { + found_time_synchronization &= cruckig_profile_check_for_second_order_velocity_with_timing_full(p, + ControlSignsUDDU, ReachedLimitsNONE, + t_profile, calc->new_phase_control[dof], + inp->max_acceleration[dof], calc->inp_min_acceleration[dof]); + } + } break; + case ControlSignsUDUD: { + if (!isinf(inp->max_jerk[dof])) { + found_time_synchronization &= cruckig_profile_check_for_velocity_with_timing_full(p, + ControlSignsUDUD, ReachedLimitsNONE, + t_profile, calc->new_phase_control[dof], + inp->max_acceleration[dof], calc->inp_min_acceleration[dof], + inp->max_jerk[dof]); + } else { + found_time_synchronization &= cruckig_profile_check_for_second_order_velocity_with_timing_full(p, + ControlSignsUDUD, ReachedLimitsNONE, + t_profile, calc->new_phase_control[dof], + inp->max_acceleration[dof], calc->inp_min_acceleration[dof]); + } + } break; + } + } break; + } + + p->limits = p_limiting->limits; /* After check method call */ + } + + if (found_time_synchronization) { + bool all_phase_or_none = true; + for (size_t dof = 0; dof < dofs; ++dof) { + if (calc->inp_per_dof_synchronization[dof] != CRuckigSyncPhase && + calc->inp_per_dof_synchronization[dof] != CRuckigSyncNone) { + all_phase_or_none = false; + break; + } + } + if (all_phase_or_none) { + return CRuckigWorking; + } + } + } + } + } + + /* Time Synchronization (Step 2) */ + for (size_t dof = 0; dof < dofs; ++dof) { + bool skip_synchronization = ((has_limiting_dof && dof == limiting_dof) || + calc->inp_per_dof_synchronization[dof] == CRuckigSyncNone) && + !discrete_duration; + if (!inp->enabled[dof] || skip_synchronization) { + continue; + } + + CRuckigProfile *p = &traj->profiles[dof]; + double t_profile = traj->duration - p->brake.duration - p->accel.duration; + + if (calc->inp_per_dof_synchronization[dof] == CRuckigSyncTimeIfNecessary && + fabs(inp->target_velocity[dof]) < eps && + fabs(inp->target_acceleration[dof]) < eps) { + *p = calc->blocks[dof].p_min; + continue; + } + + /* Check if the final time corresponds to an extremal profile from step 1 */ + if (fabs(t_profile - calc->blocks[dof].t_min) < 2 * eps) { + *p = calc->blocks[dof].p_min; + continue; + } else if (calc->blocks[dof].a.valid && fabs(t_profile - calc->blocks[dof].a.right) < 2 * eps) { + *p = calc->blocks[dof].a.profile; + continue; + } else if (calc->blocks[dof].b.valid && fabs(t_profile - calc->blocks[dof].b.right) < 2 * eps) { + *p = calc->blocks[dof].b.profile; + continue; + } + + bool found_time_synchronization = false; + switch (calc->inp_per_dof_control_interface[dof]) { + case CRuckigPosition: { + if (!isinf(inp->max_jerk[dof])) { + CRuckigPositionThirdOrderStep2 step2; + cruckig_pos3_step2_init(&step2, + t_profile, p->p[0], p->v[0], p->a[0], p->pf, p->vf, p->af, + inp->max_velocity[dof], calc->inp_min_velocity[dof], + inp->max_acceleration[dof], calc->inp_min_acceleration[dof], + inp->max_jerk[dof]); + found_time_synchronization = cruckig_pos3_step2_get_profile(&step2, p); + } else if (!isinf(inp->max_acceleration[dof])) { + CRuckigPositionSecondOrderStep2 step2; + cruckig_pos2_step2_init(&step2, + t_profile, p->p[0], p->v[0], p->pf, p->vf, + inp->max_velocity[dof], calc->inp_min_velocity[dof], + inp->max_acceleration[dof], calc->inp_min_acceleration[dof]); + found_time_synchronization = cruckig_pos2_step2_get_profile(&step2, p); + } else { + CRuckigPositionFirstOrderStep2 step2; + cruckig_pos1_step2_init(&step2, + t_profile, p->p[0], p->pf, + inp->max_velocity[dof], calc->inp_min_velocity[dof]); + found_time_synchronization = cruckig_pos1_step2_get_profile(&step2, p); + } + } break; + case CRuckigVelocity: { + if (!isinf(inp->max_jerk[dof])) { + CRuckigVelocityThirdOrderStep2 step2; + cruckig_vel3_step2_init(&step2, + t_profile, p->v[0], p->a[0], p->vf, p->af, + inp->max_acceleration[dof], calc->inp_min_acceleration[dof], + inp->max_jerk[dof]); + found_time_synchronization = cruckig_vel3_step2_get_profile(&step2, p); + } else { + CRuckigVelocitySecondOrderStep2 step2; + cruckig_vel2_step2_init(&step2, + t_profile, p->v[0], p->vf, + inp->max_acceleration[dof], calc->inp_min_acceleration[dof]); + found_time_synchronization = cruckig_vel2_step2_get_profile(&step2, p); + } + } break; + } + + if (!found_time_synchronization) { + return CRuckigErrorSynchronizationCalculation; + } + } + + return CRuckigWorking; +} + +/* + * Multi-segment waypoint calculation. + * + * Strategy: sequential segment planning. For each segment between consecutive + * waypoints, use the existing single-segment planner. The end state of segment i + * becomes the start state of segment i+1. At intermediate waypoints, velocity + * and acceleration pass through continuously (zero target velocity at waypoints + * for robustness, with option to optimize). + */ +CRuckigResult cruckig_calculator_calculate_waypoints(CRuckigCalculator *calc, + const CRuckigInputParameter *inp, + CRuckigTrajectory *traj, + double delta_time, + bool *was_interrupted) +{ + const size_t dofs = calc->degrees_of_freedom; + const size_t nwp = inp->num_intermediate_waypoints; + const size_t nsec = nwp + 1; /* Number of sections */ + + /* Resize trajectory for multi-section */ + if (!cruckig_trajectory_resize(traj, nsec)) { + return CRuckigError; + } + + /* Create reusable segment input if needed */ + if (!calc->segment_input) { + calc->segment_input = cruckig_input_create(dofs); + if (!calc->segment_input) return CRuckigError; + } + + CRuckigInputParameter *seg = calc->segment_input; + + /* Build a temporary single-section trajectory for each segment */ + CRuckigTrajectory *seg_traj = cruckig_trajectory_create(dofs); + if (!seg_traj) return CRuckigError; + + double cumulative_time = 0.0; + CRuckigResult final_result = CRuckigWorking; + + for (size_t s = 0; s < nsec; ++s) { + /* Set segment input: copy global settings */ + seg->control_interface = CRuckigPosition; + seg->synchronization = inp->synchronization; + seg->duration_discretization = CRuckigContinuous; + seg->has_minimum_duration = false; + + /* Per-section minimum duration */ + if (inp->per_section_minimum_duration) { + seg->minimum_duration = inp->per_section_minimum_duration[s]; + seg->has_minimum_duration = true; + } + + /* Set start state */ + if (s == 0) { + /* First segment starts from input current state */ + memcpy(seg->current_position, inp->current_position, dofs * sizeof(double)); + memcpy(seg->current_velocity, inp->current_velocity, dofs * sizeof(double)); + memcpy(seg->current_acceleration, inp->current_acceleration, dofs * sizeof(double)); + } + /* else: current state was set by previous iteration's end state */ + + /* Set target state */ + if (s < nwp) { + /* Target is the next intermediate waypoint */ + const double *wp = inp->intermediate_positions + s * dofs; + memcpy(seg->target_position, wp, dofs * sizeof(double)); + /* Zero velocity/acceleration at intermediate waypoints */ + memset(seg->target_velocity, 0, dofs * sizeof(double)); + memset(seg->target_acceleration, 0, dofs * sizeof(double)); + } else { + /* Last segment targets the final position */ + memcpy(seg->target_position, inp->target_position, dofs * sizeof(double)); + memcpy(seg->target_velocity, inp->target_velocity, dofs * sizeof(double)); + memcpy(seg->target_acceleration, inp->target_acceleration, dofs * sizeof(double)); + } + + /* Set kinematic constraints (per-section or global) */ + if (inp->per_section_max_velocity) { + memcpy(seg->max_velocity, inp->per_section_max_velocity + s * dofs, dofs * sizeof(double)); + } else { + memcpy(seg->max_velocity, inp->max_velocity, dofs * sizeof(double)); + } + if (inp->per_section_max_acceleration) { + memcpy(seg->max_acceleration, inp->per_section_max_acceleration + s * dofs, dofs * sizeof(double)); + } else { + memcpy(seg->max_acceleration, inp->max_acceleration, dofs * sizeof(double)); + } + if (inp->per_section_max_jerk) { + memcpy(seg->max_jerk, inp->per_section_max_jerk + s * dofs, dofs * sizeof(double)); + } else { + memcpy(seg->max_jerk, inp->max_jerk, dofs * sizeof(double)); + } + + /* Optional min limits */ + if (inp->per_section_min_velocity) { + if (!seg->min_velocity) seg->min_velocity = (double*)cruckig_malloc(dofs * sizeof(double)); + memcpy(seg->min_velocity, inp->per_section_min_velocity + s * dofs, dofs * sizeof(double)); + } else if (inp->min_velocity) { + if (!seg->min_velocity) seg->min_velocity = (double*)cruckig_malloc(dofs * sizeof(double)); + memcpy(seg->min_velocity, inp->min_velocity, dofs * sizeof(double)); + } else { + cruckig_free(seg->min_velocity); + seg->min_velocity = NULL; + } + + if (inp->per_section_min_acceleration) { + if (!seg->min_acceleration) seg->min_acceleration = (double*)cruckig_malloc(dofs * sizeof(double)); + memcpy(seg->min_acceleration, inp->per_section_min_acceleration + s * dofs, dofs * sizeof(double)); + } else if (inp->min_acceleration) { + if (!seg->min_acceleration) seg->min_acceleration = (double*)cruckig_malloc(dofs * sizeof(double)); + memcpy(seg->min_acceleration, inp->min_acceleration, dofs * sizeof(double)); + } else { + cruckig_free(seg->min_acceleration); + seg->min_acceleration = NULL; + } + + /* Enable all DOFs for segment */ + for (size_t d = 0; d < dofs; ++d) seg->enabled[d] = true; + + /* Calculate this segment */ + bool seg_interrupted = false; + CRuckigResult seg_result = cruckig_calculator_calculate(calc, seg, seg_traj, + delta_time, &seg_interrupted); + if (seg_result != CRuckigWorking) { + cruckig_trajectory_destroy(seg_traj); + *was_interrupted = false; + return seg_result; + } + + /* Copy segment profiles into the multi-section trajectory */ + double seg_duration = cruckig_trajectory_get_duration(seg_traj); + cumulative_time += seg_duration; + traj->cumulative_times[s] = cumulative_time; + + for (size_t d = 0; d < dofs; ++d) { + traj->profiles[s * dofs + d] = seg_traj->profiles[d]; + if (s == 0) { + traj->independent_min_durations[d] = seg_traj->independent_min_durations[d]; + } + } + + /* Set next segment's start state from this segment's end state */ + if (s < nsec - 1) { + for (size_t d = 0; d < dofs; ++d) { + const CRuckigProfile *p = &seg_traj->profiles[d]; + seg->current_position[d] = p->p[7]; + seg->current_velocity[d] = p->v[7]; + seg->current_acceleration[d] = p->a[7]; + } + } + } + + traj->duration = cumulative_time; + cruckig_trajectory_destroy(seg_traj); + + /* Position limits check */ + if (inp->max_position || inp->min_position || + inp->per_section_max_position || inp->per_section_min_position) + { + /* Sample trajectory and check bounds */ + double *pos = (double*)cruckig_malloc(dofs * sizeof(double)); + double *vel = (double*)cruckig_malloc(dofs * sizeof(double)); + double *acc = (double*)cruckig_malloc(dofs * sizeof(double)); + size_t sec; + + bool violated = false; + /* Check at fine time steps */ + double dt_check = (delta_time > 0.0) ? delta_time : 0.001; + for (double t = 0.0; t <= cumulative_time && !violated; t += dt_check) { + cruckig_trajectory_at_time(traj, t, pos, vel, acc, NULL, &sec); + + for (size_t d = 0; d < dofs; ++d) { + double p_max = INFINITY, p_min = -INFINITY; + + if (inp->max_position) p_max = inp->max_position[d]; + if (inp->min_position) p_min = inp->min_position[d]; + + /* Per-section position limits */ + if (sec < nsec) { + if (inp->per_section_max_position) { + double sec_max = inp->per_section_max_position[sec * dofs + d]; + if (sec_max < p_max) p_max = sec_max; + } + if (inp->per_section_min_position) { + double sec_min = inp->per_section_min_position[sec * dofs + d]; + if (sec_min > p_min) p_min = sec_min; + } + } + + if (pos[d] > p_max + 1e-8 || pos[d] < p_min - 1e-8) { + violated = true; + break; + } + } + } + + /* Also check position extrema */ + if (!violated) { + cruckig_trajectory_get_position_extrema(traj); + for (size_t d = 0; d < dofs; ++d) { + double p_max = INFINITY, p_min = -INFINITY; + if (inp->max_position) p_max = inp->max_position[d]; + if (inp->min_position) p_min = inp->min_position[d]; + + if (traj->position_extrema[d].max > p_max + 1e-8 || + traj->position_extrema[d].min < p_min - 1e-8) { + violated = true; + break; + } + } + } + + cruckig_free(pos); + cruckig_free(vel); + cruckig_free(acc); + + if (violated) { + final_result = CRuckigErrorPositionalLimits; + } + } + + *was_interrupted = false; + return final_result; +} + +CRuckigResult cruckig_calculator_continue(CRuckigCalculator *calc, + const CRuckigInputParameter *inp, + CRuckigTrajectory *traj, + double delta_time, + bool *was_interrupted) +{ + /* For now, continue_calculation simply re-runs the full calculation. + * A future optimization could resume from partial state. */ + if (inp->num_intermediate_waypoints > 0 && inp->control_interface == CRuckigPosition) { + return cruckig_calculator_calculate_waypoints(calc, inp, traj, delta_time, was_interrupted); + } + return cruckig_calculator_calculate(calc, inp, traj, delta_time, was_interrupted); +} diff --git a/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/calculator.h b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/calculator.h new file mode 100644 index 0000000..51e69e7 --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/calculator.h @@ -0,0 +1,71 @@ +/* + * cruckig - Pure C99 port of the Ruckig trajectory generation library + * Copyright (c) 2025 Yang Yang + * Copyright (c) 2021 Lars Berscheid (original C++ Ruckig) + * + * License: MIT, see the LICENSE file in this directory. + */ +#ifndef CRUCKIG_CALCULATOR_H +#define CRUCKIG_CALCULATOR_H + +#include "cruckig_internal.h" +#include "result.h" +#include "block.h" +#include "input_parameter.h" +#include "trajectory.h" +#include "position.h" +#include "velocity.h" + +typedef struct { + size_t degrees_of_freedom; + + double *new_phase_control; + double *pd; + double *possible_t_syncs; + size_t *idx; + + CRuckigBlock *blocks; + double *inp_min_velocity; + double *inp_min_acceleration; + CRuckigControlInterface *inp_per_dof_control_interface; + CRuckigSynchronization *inp_per_dof_synchronization; + + /* Scratch space for waypoint calculation */ + CRuckigInputParameter *segment_input; /* Reusable per-segment input */ + + /* Step1 workspace: kept off the stack to stay within kernel frame limits. + * Only one Step1 type is active at a time, so a union suffices. */ + union { + CRuckigPositionThirdOrderStep1 pos3_step1; + CRuckigPositionSecondOrderStep1 pos2_step1; + CRuckigPositionFirstOrderStep1 pos1_step1; + CRuckigVelocityThirdOrderStep1 vel3_step1; + CRuckigVelocitySecondOrderStep1 vel2_step1; + } step1_workspace; +} CRuckigCalculator; + +CRuckigCalculator* cruckig_calculator_create(size_t dofs); +void cruckig_calculator_destroy(CRuckigCalculator *calc); + +/* Single-segment calculation (existing, backward compatible) */ +CRuckigResult cruckig_calculator_calculate(CRuckigCalculator *calc, + const CRuckigInputParameter *inp, + CRuckigTrajectory *traj, + double delta_time, + bool *was_interrupted); + +/* Multi-segment waypoint calculation */ +CRuckigResult cruckig_calculator_calculate_waypoints(CRuckigCalculator *calc, + const CRuckigInputParameter *inp, + CRuckigTrajectory *traj, + double delta_time, + bool *was_interrupted); + +/* Continue an interrupted calculation */ +CRuckigResult cruckig_calculator_continue(CRuckigCalculator *calc, + const CRuckigInputParameter *inp, + CRuckigTrajectory *traj, + double delta_time, + bool *was_interrupted); + +#endif /* CRUCKIG_CALCULATOR_H */ diff --git a/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/cruckig.c b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/cruckig.c new file mode 100644 index 0000000..abe799d --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/cruckig.c @@ -0,0 +1,173 @@ +/* + * cruckig - Pure C99 port of the Ruckig trajectory generation library + * Copyright (c) 2025 Yang Yang + * Copyright (c) 2021 Lars Berscheid (original C++ Ruckig) + * + * License: MIT, see the LICENSE file in this directory. + */ + +#include "cruckig.h" + + +static CRuckig* cruckig_create_internal(size_t dofs, double delta_time, size_t max_waypoints) { + CRuckig *r = (CRuckig*)cruckig_calloc(1, sizeof(CRuckig)); + if (!r) return NULL; + + r->degrees_of_freedom = dofs; + r->delta_time = delta_time; + r->max_number_of_waypoints = max_waypoints; + + r->calculator = cruckig_calculator_create(dofs); + if (!r->calculator) { + cruckig_free(r); + return NULL; + } + + r->current_input = cruckig_input_create(dofs); + if (!r->current_input) { + cruckig_calculator_destroy(r->calculator); + cruckig_free(r); + return NULL; + } + + r->current_input_initialized = false; + + return r; +} + +CRuckig* cruckig_create(size_t dofs, double delta_time) { + return cruckig_create_internal(dofs, delta_time, 0); +} + +CRuckig* cruckig_create_waypoints(size_t dofs, double delta_time, size_t max_waypoints) { + return cruckig_create_internal(dofs, delta_time, max_waypoints); +} + +void cruckig_destroy(CRuckig *r) { + if (!r) return; + cruckig_calculator_destroy(r->calculator); + cruckig_input_destroy(r->current_input); + cruckig_free(r); +} + +void cruckig_reset(CRuckig *r) { + if (!r) return; + r->current_input_initialized = false; +} + +static inline bool use_waypoints(const CRuckigInputParameter *input) { + return input->num_intermediate_waypoints > 0 && + input->control_interface == CRuckigPosition; +} + +bool cruckig_validate_input(const CRuckig *r, const CRuckigInputParameter *input, + bool check_current_within_limits, + bool check_target_within_limits) +{ + if (!r || !input) return false; + + if (!cruckig_input_validate(input, check_current_within_limits, check_target_within_limits)) { + return false; + } + + if (r->delta_time <= 0.0 && input->duration_discretization != CRuckigContinuous) { + return false; + } + + /* Validate waypoint count against max */ + if (input->num_intermediate_waypoints > r->max_number_of_waypoints && + r->max_number_of_waypoints > 0) { + return false; + } + + return true; +} + +static CRuckigResult dispatch_calculate(CRuckig *r, const CRuckigInputParameter *input, + CRuckigTrajectory *trajectory, bool *was_interrupted) +{ + if (use_waypoints(input)) { + /* Ensure trajectory has enough capacity */ + size_t nsec = input->num_intermediate_waypoints + 1; + if (!cruckig_trajectory_resize(trajectory, nsec)) { + return CRuckigError; + } + return cruckig_calculator_calculate_waypoints(r->calculator, input, trajectory, + r->delta_time, was_interrupted); + } else { + /* Single-segment: ensure single section */ + if (trajectory->num_sections != 1) { + cruckig_trajectory_resize(trajectory, 1); + } + return cruckig_calculator_calculate(r->calculator, input, trajectory, + r->delta_time, was_interrupted); + } +} + +CRuckigResult cruckig_calculate(CRuckig *r, const CRuckigInputParameter *input, + CRuckigTrajectory *trajectory) +{ + if (!r || !input || !trajectory) return CRuckigError; + + if (!cruckig_validate_input(r, input, false, true)) { + return CRuckigErrorInvalidInput; + } + + bool was_interrupted = false; + return dispatch_calculate(r, input, trajectory, &was_interrupted); +} + +static double get_time_us(void) { + /* Timing measurement for interrupt budget feature. + * Not used by LinuxCNC (only cruckig_update, not cruckig_calculate). */ + return 0.0; +} + +CRUCKIG_HOT +CRuckigResult cruckig_update(CRuckig *r, const CRuckigInputParameter *input, + CRuckigOutputParameter *output) +{ + if (CRUCKIG_UNLIKELY(!r || !input || !output)) return CRuckigError; + + double start_us = get_time_us(); + + output->new_calculation = false; + + CRuckigResult result = CRuckigWorking; + if (!r->current_input_initialized || !cruckig_input_is_equal(input, r->current_input)) { + if (!cruckig_validate_input(r, input, false, true)) { + return CRuckigErrorInvalidInput; + } + + result = dispatch_calculate(r, input, output->trajectory, + &output->was_calculation_interrupted); + if (result != CRuckigWorking && result != CRuckigErrorPositionalLimits) { + return result; + } + + cruckig_input_copy(r->current_input, input); + r->current_input_initialized = true; + output->time = 0.0; + output->new_section = 0; + output->new_calculation = true; + } + + size_t old_section = output->new_section; + output->time += r->delta_time; + cruckig_trajectory_at_time(output->trajectory, output->time, + output->new_position, output->new_velocity, + output->new_acceleration, output->new_jerk, + &output->new_section); + output->did_section_change = (output->new_section > old_section); + + double stop_us = get_time_us(); + output->calculation_duration = stop_us - start_us; + + cruckig_output_pass_to_input(output, r->current_input); + + if (output->time > cruckig_trajectory_get_duration(output->trajectory)) { + return CRuckigFinished; + } + + return result; +} diff --git a/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/cruckig.h b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/cruckig.h new file mode 100644 index 0000000..55d748a --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/cruckig.h @@ -0,0 +1,54 @@ +/* + * cruckig - Pure C99 port of the Ruckig trajectory generation library + * Copyright (c) 2025 Yang Yang + * Copyright (c) 2021 Lars Berscheid (original C++ Ruckig) + * + * License: MIT, see the LICENSE file in this directory. + */ +#ifndef CRUCKIG_CRUCKIG_H +#define CRUCKIG_CRUCKIG_H + +#include "cruckig_internal.h" + +#include "result.h" +#include "input_parameter.h" +#include "output_parameter.h" +#include "trajectory.h" +#include "calculator.h" + +/* Main cruckig instance */ +typedef struct { + size_t degrees_of_freedom; + double delta_time; + size_t max_number_of_waypoints; + + CRuckigCalculator *calculator; + CRuckigInputParameter *current_input; + bool current_input_initialized; +} CRuckig; + +/* Create and destroy (backward compatible: 0 waypoints) */ +CRuckig* cruckig_create(size_t dofs, double delta_time); + +/* Create with waypoint support */ +CRuckig* cruckig_create_waypoints(size_t dofs, double delta_time, size_t max_waypoints); + +void cruckig_destroy(CRuckig *r); + +/* Reset (force recalculation on next update) */ +void cruckig_reset(CRuckig *r); + +/* Calculate trajectory (offline, auto-dispatches to waypoint calculator if needed) */ +CRuckigResult cruckig_calculate(CRuckig *r, const CRuckigInputParameter *input, + CRuckigTrajectory *trajectory); + +/* Update (online, call every delta_time) */ +CRuckigResult cruckig_update(CRuckig *r, const CRuckigInputParameter *input, + CRuckigOutputParameter *output); + +/* Validate input */ +bool cruckig_validate_input(const CRuckig *r, const CRuckigInputParameter *input, + bool check_current_within_limits, + bool check_target_within_limits); + +#endif /* CRUCKIG_CRUCKIG_H */ diff --git a/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/cruckig_internal.h b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/cruckig_internal.h new file mode 100644 index 0000000..8a442bf --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/cruckig_internal.h @@ -0,0 +1,55 @@ +/* + * cruckig_internal.h - Internal header for cruckig + * Copyright (c) 2025 Yang Yang + * Copyright (c) 2021 Lars Berscheid (original C++ Ruckig) + * + * License: MIT, see the LICENSE file in this directory. + * + * Provides RTAPI-portable types, memory allocation, math, string + * functions, and compiler hint macros for cruckig internals. + * + * All cruckig headers should include this as their first include. + * C files should NOT include this directly -- they get it through + * their corresponding header. + */ +#ifndef CRUCKIG_CRUCKIG_INTERNAL_H +#define CRUCKIG_CRUCKIG_INTERNAL_H + +/* RTAPI provides bool, size_t, math, string, and memory allocation + * portably across userspace and kernel builds. */ +#include +#include +#include +#include +#include +#include + +/* INFINITY: not provided by rtapi_math.h in kernel space */ +#ifndef INFINITY +#define INFINITY __builtin_inf() +#endif + +/* Memory allocation: always use rtapi_slab wrappers */ +#define cruckig_malloc(sz) rtapi_kmalloc(sz, RTAPI_GFP_KERNEL) +#define cruckig_calloc(n, sz) rtapi_kzalloc((n) * (sz), RTAPI_GFP_KERNEL) +#define cruckig_realloc(p, sz) rtapi_krealloc(p, sz, RTAPI_GFP_KERNEL) +#define cruckig_free(p) rtapi_kfree(p) + +/* Branch prediction hints */ +#if defined(__GNUC__) || defined(__clang__) +# define CRUCKIG_LIKELY(x) __builtin_expect(!!(x), 1) +# define CRUCKIG_UNLIKELY(x) __builtin_expect(!!(x), 0) +# define CRUCKIG_FORCE_INLINE static inline __attribute__((always_inline)) +# define CRUCKIG_HOT __attribute__((hot)) +# define CRUCKIG_RESTRICT __restrict__ +# define CRUCKIG_PREFETCH(addr) __builtin_prefetch(addr, 0, 1) +#else +# define CRUCKIG_LIKELY(x) (x) +# define CRUCKIG_UNLIKELY(x) (x) +# define CRUCKIG_FORCE_INLINE static inline +# define CRUCKIG_HOT +# define CRUCKIG_RESTRICT restrict +# define CRUCKIG_PREFETCH(addr) ((void)0) +#endif + +#endif /* CRUCKIG_CRUCKIG_INTERNAL_H */ diff --git a/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/input_parameter.c b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/input_parameter.c new file mode 100644 index 0000000..2b2897d --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/input_parameter.c @@ -0,0 +1,408 @@ +/* + * cruckig - Pure C99 port of the Ruckig trajectory generation library + * Copyright (c) 2025 Yang Yang + * Copyright (c) 2021 Lars Berscheid (original C++ Ruckig) + * + * License: MIT, see the LICENSE file in this directory. + */ +#include "input_parameter.h" + + +static double v_at_a_zero(double v0, double a0, double j) { + return v0 + (a0 * a0) / (2.0 * j); +} + +CRuckigInputParameter* cruckig_input_create(size_t dofs) { + CRuckigInputParameter *inp = (CRuckigInputParameter*)cruckig_calloc(1, sizeof(CRuckigInputParameter)); + if (!inp) return NULL; + + inp->degrees_of_freedom = dofs; + inp->control_interface = CRuckigPosition; + inp->synchronization = CRuckigSyncTime; + inp->duration_discretization = CRuckigContinuous; + + inp->current_position = (double*)cruckig_calloc(dofs, sizeof(double)); + inp->current_velocity = (double*)cruckig_calloc(dofs, sizeof(double)); + inp->current_acceleration = (double*)cruckig_calloc(dofs, sizeof(double)); + inp->target_position = (double*)cruckig_calloc(dofs, sizeof(double)); + inp->target_velocity = (double*)cruckig_calloc(dofs, sizeof(double)); + inp->target_acceleration = (double*)cruckig_calloc(dofs, sizeof(double)); + inp->max_velocity = (double*)cruckig_calloc(dofs, sizeof(double)); + inp->max_acceleration = (double*)cruckig_malloc(dofs * sizeof(double)); + inp->max_jerk = (double*)cruckig_malloc(dofs * sizeof(double)); + inp->enabled = (bool*)cruckig_malloc(dofs * sizeof(bool)); + + if (!inp->current_position || !inp->current_velocity || !inp->current_acceleration || + !inp->target_position || !inp->target_velocity || !inp->target_acceleration || + !inp->max_velocity || !inp->max_acceleration || !inp->max_jerk || !inp->enabled) { + cruckig_input_destroy(inp); + return NULL; + } + + /* Initialize defaults matching C++ */ + for (size_t dof = 0; dof < dofs; ++dof) { + inp->max_acceleration[dof] = INFINITY; + inp->max_jerk[dof] = INFINITY; + inp->enabled[dof] = true; + } + + inp->min_velocity = NULL; + inp->min_acceleration = NULL; + inp->per_dof_control_interface = NULL; + inp->per_dof_synchronization = NULL; + inp->minimum_duration = -1.0; + inp->has_minimum_duration = false; + + /* Pro fields: initialize to defaults */ + inp->intermediate_positions = NULL; + inp->num_intermediate_waypoints = 0; + inp->per_section_max_velocity = NULL; + inp->per_section_max_acceleration = NULL; + inp->per_section_max_jerk = NULL; + inp->per_section_min_velocity = NULL; + inp->per_section_min_acceleration = NULL; + inp->per_section_max_position = NULL; + inp->per_section_min_position = NULL; + inp->max_position = NULL; + inp->min_position = NULL; + inp->per_section_minimum_duration = NULL; + inp->interrupt_calculation_duration = 0.0; + + return inp; +} + +void cruckig_input_destroy(CRuckigInputParameter *inp) { + if (!inp) return; + cruckig_free(inp->current_position); + cruckig_free(inp->current_velocity); + cruckig_free(inp->current_acceleration); + cruckig_free(inp->target_position); + cruckig_free(inp->target_velocity); + cruckig_free(inp->target_acceleration); + cruckig_free(inp->max_velocity); + cruckig_free(inp->max_acceleration); + cruckig_free(inp->max_jerk); + cruckig_free(inp->enabled); + cruckig_free(inp->min_velocity); + cruckig_free(inp->min_acceleration); + cruckig_free(inp->per_dof_control_interface); + cruckig_free(inp->per_dof_synchronization); + /* Pro fields */ + cruckig_free(inp->intermediate_positions); + cruckig_free(inp->per_section_max_velocity); + cruckig_free(inp->per_section_max_acceleration); + cruckig_free(inp->per_section_max_jerk); + cruckig_free(inp->per_section_min_velocity); + cruckig_free(inp->per_section_min_acceleration); + cruckig_free(inp->per_section_max_position); + cruckig_free(inp->per_section_min_position); + cruckig_free(inp->max_position); + cruckig_free(inp->min_position); + cruckig_free(inp->per_section_minimum_duration); + cruckig_free(inp); +} + +void cruckig_input_set_intermediate_positions(CRuckigInputParameter *inp, + const double *positions, + size_t num_waypoints) +{ + if (!inp) return; + const size_t dofs = inp->degrees_of_freedom; + + cruckig_free(inp->intermediate_positions); + if (num_waypoints == 0 || !positions) { + inp->intermediate_positions = NULL; + inp->num_intermediate_waypoints = 0; + return; + } + + size_t total = num_waypoints * dofs; + inp->intermediate_positions = (double*)cruckig_malloc(total * sizeof(double)); + memcpy(inp->intermediate_positions, positions, total * sizeof(double)); + inp->num_intermediate_waypoints = num_waypoints; +} + +bool cruckig_input_validate(const CRuckigInputParameter *inp, + bool check_current_within_limits, + bool check_target_within_limits) +{ + if (!inp) return false; + const size_t dofs = inp->degrees_of_freedom; + + /* Waypoint-specific validation */ + if (inp->num_intermediate_waypoints > 0) { + /* Waypoints require Position control interface */ + if (inp->control_interface != CRuckigPosition) return false; + /* Waypoints incompatible with Discrete discretization */ + if (inp->duration_discretization == CRuckigDiscrete) return false; + /* Waypoints incompatible with minimum_duration */ + if (inp->has_minimum_duration) return false; + + /* Infinite jerk not supported with waypoints */ + for (size_t dof = 0; dof < dofs; ++dof) { + if (isinf(inp->max_jerk[dof])) return false; + if (isinf(inp->max_acceleration[dof])) return false; + } + } + + for (size_t dof = 0; dof < dofs; ++dof) { + const double jMax = inp->max_jerk[dof]; + if (isnan(jMax) || jMax < 0.0) return false; + + const double aMax = inp->max_acceleration[dof]; + if (isnan(aMax) || aMax < 0.0) return false; + + const double aMin = inp->min_acceleration ? inp->min_acceleration[dof] : -aMax; + if (isnan(aMin) || aMin > 0.0) return false; + + const double a0 = inp->current_acceleration[dof]; + if (isnan(a0)) return false; + const double af = inp->target_acceleration[dof]; + if (isnan(af)) return false; + + if (check_current_within_limits) { + if (a0 > aMax) return false; + if (a0 < aMin) return false; + } + if (check_target_within_limits) { + if (af > aMax) return false; + if (af < aMin) return false; + } + + const double v0 = inp->current_velocity[dof]; + if (isnan(v0)) return false; + const double vf = inp->target_velocity[dof]; + if (isnan(vf)) return false; + + CRuckigControlInterface ci = inp->per_dof_control_interface + ? inp->per_dof_control_interface[dof] + : inp->control_interface; + + if (ci == CRuckigPosition) { + const double p0 = inp->current_position[dof]; + if (isnan(p0)) return false; + const double pf = inp->target_position[dof]; + if (isnan(pf)) return false; + + const double vMax = inp->max_velocity[dof]; + if (isnan(vMax) || vMax < 0.0) return false; + + const double vMin = inp->min_velocity ? inp->min_velocity[dof] : -vMax; + if (isnan(vMin) || vMin > 0.0) return false; + + if (check_current_within_limits) { + if (v0 > vMax) return false; + if (v0 < vMin) return false; + } + if (check_target_within_limits) { + if (vf > vMax) return false; + if (vf < vMin) return false; + } + + if (check_current_within_limits) { + if (a0 > 0 && jMax > 0 && v_at_a_zero(v0, a0, jMax) > vMax) + return false; + if (a0 < 0 && jMax > 0 && v_at_a_zero(v0, a0, -jMax) < vMin) + return false; + } + if (check_target_within_limits) { + if (af < 0 && jMax > 0 && v_at_a_zero(vf, af, jMax) > vMax) + return false; + if (af > 0 && jMax > 0 && v_at_a_zero(vf, af, -jMax) < vMin) + return false; + } + } + } + + return true; +} + +bool cruckig_input_is_equal(const CRuckigInputParameter *a, const CRuckigInputParameter *b) { + if (!a || !b) return (a == b); + if (a->degrees_of_freedom != b->degrees_of_freedom) return false; + + const size_t dofs = a->degrees_of_freedom; + const size_t dsz = dofs * sizeof(double); + + if (memcmp(a->current_position, b->current_position, dsz) != 0) return false; + if (memcmp(a->current_velocity, b->current_velocity, dsz) != 0) return false; + if (memcmp(a->current_acceleration, b->current_acceleration, dsz) != 0) return false; + if (memcmp(a->target_position, b->target_position, dsz) != 0) return false; + if (memcmp(a->target_velocity, b->target_velocity, dsz) != 0) return false; + if (memcmp(a->target_acceleration, b->target_acceleration, dsz) != 0) return false; + if (memcmp(a->max_velocity, b->max_velocity, dsz) != 0) return false; + if (memcmp(a->max_acceleration, b->max_acceleration, dsz) != 0) return false; + if (memcmp(a->max_jerk, b->max_jerk, dsz) != 0) return false; + + if (memcmp(a->enabled, b->enabled, dofs * sizeof(bool)) != 0) return false; + + /* Compare optional min_velocity */ + if ((a->min_velocity == NULL) != (b->min_velocity == NULL)) return false; + if (a->min_velocity && memcmp(a->min_velocity, b->min_velocity, dsz) != 0) return false; + + /* Compare optional min_acceleration */ + if ((a->min_acceleration == NULL) != (b->min_acceleration == NULL)) return false; + if (a->min_acceleration && memcmp(a->min_acceleration, b->min_acceleration, dsz) != 0) return false; + + /* Compare optional per_dof_control_interface */ + if ((a->per_dof_control_interface == NULL) != (b->per_dof_control_interface == NULL)) return false; + if (a->per_dof_control_interface && + memcmp(a->per_dof_control_interface, b->per_dof_control_interface, + dofs * sizeof(CRuckigControlInterface)) != 0) return false; + + /* Compare optional per_dof_synchronization */ + if ((a->per_dof_synchronization == NULL) != (b->per_dof_synchronization == NULL)) return false; + if (a->per_dof_synchronization && + memcmp(a->per_dof_synchronization, b->per_dof_synchronization, + dofs * sizeof(CRuckigSynchronization)) != 0) return false; + + if (a->control_interface != b->control_interface) return false; + if (a->synchronization != b->synchronization) return false; + if (a->duration_discretization != b->duration_discretization) return false; + + if (a->has_minimum_duration != b->has_minimum_duration) return false; + if (a->has_minimum_duration && a->minimum_duration != b->minimum_duration) return false; + + /* Compare Pro fields */ + if (a->num_intermediate_waypoints != b->num_intermediate_waypoints) return false; + if (a->num_intermediate_waypoints > 0) { + size_t wp_sz = a->num_intermediate_waypoints * dofs * sizeof(double); + if (memcmp(a->intermediate_positions, b->intermediate_positions, wp_sz) != 0) return false; + } + + /* Compare position limits */ + if ((a->max_position == NULL) != (b->max_position == NULL)) return false; + if (a->max_position && memcmp(a->max_position, b->max_position, dsz) != 0) return false; + if ((a->min_position == NULL) != (b->min_position == NULL)) return false; + if (a->min_position && memcmp(a->min_position, b->min_position, dsz) != 0) return false; + + /* Compare per-section constraints */ + size_t nsec = a->num_intermediate_waypoints + 1; + size_t sec_dsz = nsec * dofs * sizeof(double); + +#define CMP_OPT_SEC(field) \ + if ((a->field == NULL) != (b->field == NULL)) return false; \ + if (a->field && memcmp(a->field, b->field, sec_dsz) != 0) return false; + + CMP_OPT_SEC(per_section_max_velocity) + CMP_OPT_SEC(per_section_max_acceleration) + CMP_OPT_SEC(per_section_max_jerk) + CMP_OPT_SEC(per_section_min_velocity) + CMP_OPT_SEC(per_section_min_acceleration) + CMP_OPT_SEC(per_section_max_position) + CMP_OPT_SEC(per_section_min_position) +#undef CMP_OPT_SEC + + if ((a->per_section_minimum_duration == NULL) != (b->per_section_minimum_duration == NULL)) return false; + if (a->per_section_minimum_duration && + memcmp(a->per_section_minimum_duration, b->per_section_minimum_duration, + nsec * sizeof(double)) != 0) return false; + + if (a->interrupt_calculation_duration != b->interrupt_calculation_duration) return false; + + return true; +} + +/* Helper to copy an optional flat array */ +static void copy_opt_array(double **dst, const double *src, size_t count) { + if (src) { + size_t sz = count * sizeof(double); + if (!*dst) { + *dst = (double*)cruckig_malloc(sz); + } + memcpy(*dst, src, sz); + } else { + cruckig_free(*dst); + *dst = NULL; + } +} + +void cruckig_input_copy(CRuckigInputParameter *dst, const CRuckigInputParameter *src) { + if (!dst || !src) return; + if (dst == src) return; + + const size_t dofs = src->degrees_of_freedom; + const size_t dsz = dofs * sizeof(double); + + /* dst must already be allocated with same dofs */ + dst->degrees_of_freedom = dofs; + dst->control_interface = src->control_interface; + dst->synchronization = src->synchronization; + dst->duration_discretization = src->duration_discretization; + + memcpy(dst->current_position, src->current_position, dsz); + memcpy(dst->current_velocity, src->current_velocity, dsz); + memcpy(dst->current_acceleration, src->current_acceleration, dsz); + memcpy(dst->target_position, src->target_position, dsz); + memcpy(dst->target_velocity, src->target_velocity, dsz); + memcpy(dst->target_acceleration, src->target_acceleration, dsz); + memcpy(dst->max_velocity, src->max_velocity, dsz); + memcpy(dst->max_acceleration, src->max_acceleration, dsz); + memcpy(dst->max_jerk, src->max_jerk, dsz); + memcpy(dst->enabled, src->enabled, dofs * sizeof(bool)); + + copy_opt_array(&dst->min_velocity, src->min_velocity, dofs); + copy_opt_array(&dst->min_acceleration, src->min_acceleration, dofs); + + /* Handle optional per_dof_control_interface */ + if (src->per_dof_control_interface) { + if (!dst->per_dof_control_interface) { + dst->per_dof_control_interface = (CRuckigControlInterface*)cruckig_malloc(dofs * sizeof(CRuckigControlInterface)); + } + memcpy(dst->per_dof_control_interface, src->per_dof_control_interface, + dofs * sizeof(CRuckigControlInterface)); + } else { + cruckig_free(dst->per_dof_control_interface); + dst->per_dof_control_interface = NULL; + } + + /* Handle optional per_dof_synchronization */ + if (src->per_dof_synchronization) { + if (!dst->per_dof_synchronization) { + dst->per_dof_synchronization = (CRuckigSynchronization*)cruckig_malloc(dofs * sizeof(CRuckigSynchronization)); + } + memcpy(dst->per_dof_synchronization, src->per_dof_synchronization, + dofs * sizeof(CRuckigSynchronization)); + } else { + cruckig_free(dst->per_dof_synchronization); + dst->per_dof_synchronization = NULL; + } + + dst->minimum_duration = src->minimum_duration; + dst->has_minimum_duration = src->has_minimum_duration; + + /* Copy Pro fields */ + if (src->num_intermediate_waypoints > 0 && src->intermediate_positions) { + size_t wp_sz = src->num_intermediate_waypoints * dofs; + copy_opt_array(&dst->intermediate_positions, src->intermediate_positions, wp_sz); + dst->num_intermediate_waypoints = src->num_intermediate_waypoints; + } else { + cruckig_free(dst->intermediate_positions); + dst->intermediate_positions = NULL; + dst->num_intermediate_waypoints = 0; + } + + copy_opt_array(&dst->max_position, src->max_position, dofs); + copy_opt_array(&dst->min_position, src->min_position, dofs); + + /* Per-section arrays */ + size_t nsec = src->num_intermediate_waypoints + 1; + size_t sec_count = nsec * dofs; + + copy_opt_array(&dst->per_section_max_velocity, src->per_section_max_velocity, sec_count); + copy_opt_array(&dst->per_section_max_acceleration, src->per_section_max_acceleration, sec_count); + copy_opt_array(&dst->per_section_max_jerk, src->per_section_max_jerk, sec_count); + copy_opt_array(&dst->per_section_min_velocity, src->per_section_min_velocity, sec_count); + copy_opt_array(&dst->per_section_min_acceleration, src->per_section_min_acceleration, sec_count); + copy_opt_array(&dst->per_section_max_position, src->per_section_max_position, sec_count); + copy_opt_array(&dst->per_section_min_position, src->per_section_min_position, sec_count); + + if (src->per_section_minimum_duration) { + copy_opt_array(&dst->per_section_minimum_duration, src->per_section_minimum_duration, nsec); + } else { + cruckig_free(dst->per_section_minimum_duration); + dst->per_section_minimum_duration = NULL; + } + + dst->interrupt_calculation_duration = src->interrupt_calculation_duration; +} diff --git a/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/input_parameter.h b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/input_parameter.h new file mode 100644 index 0000000..5a9c9c6 --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/input_parameter.h @@ -0,0 +1,94 @@ +/* + * cruckig - Pure C99 port of the Ruckig trajectory generation library + * Copyright (c) 2025 Yang Yang + * Copyright (c) 2021 Lars Berscheid (original C++ Ruckig) + * + * License: MIT, see the LICENSE file in this directory. + */ +#ifndef CRUCKIG_INPUT_PARAMETER_H +#define CRUCKIG_INPUT_PARAMETER_H + +#include "cruckig_internal.h" +#include "result.h" + +typedef struct { + size_t degrees_of_freedom; + + CRuckigControlInterface control_interface; + CRuckigSynchronization synchronization; + CRuckigDurationDiscretization duration_discretization; + + /* Current state */ + double *current_position; + double *current_velocity; + double *current_acceleration; + + /* Target state */ + double *target_position; + double *target_velocity; + double *target_acceleration; + + /* Kinematic constraints */ + double *max_velocity; + double *max_acceleration; + double *max_jerk; + + /* Optional min limits (NULL = use -max) */ + double *min_velocity; /* NULL or array of dofs */ + double *min_acceleration; /* NULL or array of dofs */ + + /* Per-DOF enable flags */ + bool *enabled; + + /* Optional per-DOF control interface / synchronization (NULL = use global) */ + CRuckigControlInterface *per_dof_control_interface; /* NULL or array of dofs */ + CRuckigSynchronization *per_dof_synchronization; /* NULL or array of dofs */ + + /* Optional minimum trajectory duration (-1 = not set) */ + double minimum_duration; + bool has_minimum_duration; + + /* ---- Pro features ---- */ + + /* Intermediate waypoints: flat array of num_waypoints * dofs doubles. + * Each waypoint is dofs consecutive doubles. NULL if no waypoints. */ + double *intermediate_positions; + size_t num_intermediate_waypoints; + + /* Per-section kinematic constraints: flat arrays of (num_waypoints+1) * dofs. + * Section i constraints at offset i*dofs. NULL = use global. */ + double *per_section_max_velocity; + double *per_section_max_acceleration; + double *per_section_max_jerk; + double *per_section_min_velocity; + double *per_section_min_acceleration; + + /* Per-section position limits: flat arrays of (num_waypoints+1) * dofs. */ + double *per_section_max_position; + double *per_section_min_position; + + /* Global position limits during trajectory (NULL = no limits) */ + double *max_position; /* NULL or array of dofs */ + double *min_position; /* NULL or array of dofs */ + + /* Per-section minimum duration: array of (num_waypoints+1). NULL = no constraint. */ + double *per_section_minimum_duration; + + /* Calculation interruption budget in microseconds. 0 = no interruption. */ + double interrupt_calculation_duration; +} CRuckigInputParameter; + +CRuckigInputParameter* cruckig_input_create(size_t dofs); +void cruckig_input_destroy(CRuckigInputParameter *inp); +bool cruckig_input_validate(const CRuckigInputParameter *inp, + bool check_current_within_limits, + bool check_target_within_limits); +bool cruckig_input_is_equal(const CRuckigInputParameter *a, const CRuckigInputParameter *b); +void cruckig_input_copy(CRuckigInputParameter *dst, const CRuckigInputParameter *src); + +/* Set intermediate waypoints. Copies the data. positions is num_waypoints * dofs doubles. */ +void cruckig_input_set_intermediate_positions(CRuckigInputParameter *inp, + const double *positions, + size_t num_waypoints); + +#endif /* CRUCKIG_INPUT_PARAMETER_H */ diff --git a/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/output_parameter.c b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/output_parameter.c new file mode 100644 index 0000000..d0e1bb7 --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/output_parameter.c @@ -0,0 +1,104 @@ +/* + * cruckig - Pure C99 port of the Ruckig trajectory generation library + * Copyright (c) 2025 Yang Yang + * Copyright (c) 2021 Lars Berscheid (original C++ Ruckig) + * + * License: MIT, see the LICENSE file in this directory. + */ +#include "output_parameter.h" + + +CRuckigOutputParameter* cruckig_output_create(size_t dofs) { + CRuckigOutputParameter *out = (CRuckigOutputParameter*)cruckig_calloc(1, sizeof(CRuckigOutputParameter)); + if (!out) return NULL; + + out->degrees_of_freedom = dofs; + + out->trajectory = cruckig_trajectory_create(dofs); + if (!out->trajectory) { + cruckig_free(out); + return NULL; + } + + out->new_position = (double*)cruckig_calloc(dofs, sizeof(double)); + out->new_velocity = (double*)cruckig_calloc(dofs, sizeof(double)); + out->new_acceleration = (double*)cruckig_calloc(dofs, sizeof(double)); + out->new_jerk = (double*)cruckig_calloc(dofs, sizeof(double)); + + if (!out->new_position || !out->new_velocity || + !out->new_acceleration || !out->new_jerk) { + cruckig_output_destroy(out); + return NULL; + } + + out->time = 0.0; + out->new_section = 0; + out->did_section_change = false; + out->new_calculation = false; + out->was_calculation_interrupted = false; + out->calculation_duration = 0.0; + + return out; +} + +void cruckig_output_destroy(CRuckigOutputParameter *out) { + if (!out) return; + cruckig_trajectory_destroy(out->trajectory); + cruckig_free(out->new_position); + cruckig_free(out->new_velocity); + cruckig_free(out->new_acceleration); + cruckig_free(out->new_jerk); + cruckig_free(out); +} + +void cruckig_output_pass_to_input(const CRuckigOutputParameter *out, CRuckigInputParameter *inp) { + if (!out || !inp) return; + + const size_t dofs = out->degrees_of_freedom; + const size_t dsz = dofs * sizeof(double); + + memcpy(inp->current_position, out->new_position, dsz); + memcpy(inp->current_velocity, out->new_velocity, dsz); + memcpy(inp->current_acceleration, out->new_acceleration, dsz); + + /* If section changed and we have intermediate waypoints, remove the first waypoint */ + if (out->did_section_change && inp->num_intermediate_waypoints > 0) { + size_t remaining = inp->num_intermediate_waypoints - 1; + if (remaining == 0) { + cruckig_free(inp->intermediate_positions); + inp->intermediate_positions = NULL; + inp->num_intermediate_waypoints = 0; + } else { + /* Shift waypoints forward by one */ + memmove(inp->intermediate_positions, + inp->intermediate_positions + dofs, + remaining * dofs * sizeof(double)); + inp->num_intermediate_waypoints = remaining; + } + + /* Also shift per-section constraints if present */ + size_t old_nsec = remaining + 2; /* was num_waypoints+1 sections */ + size_t new_nsec = remaining + 1; + +#define SHIFT_PER_SEC(field) \ + if (inp->field) { \ + memmove(inp->field, inp->field + dofs, new_nsec * dofs * sizeof(double)); \ + } + + SHIFT_PER_SEC(per_section_max_velocity) + SHIFT_PER_SEC(per_section_max_acceleration) + SHIFT_PER_SEC(per_section_max_jerk) + SHIFT_PER_SEC(per_section_min_velocity) + SHIFT_PER_SEC(per_section_min_acceleration) + SHIFT_PER_SEC(per_section_max_position) + SHIFT_PER_SEC(per_section_min_position) +#undef SHIFT_PER_SEC + + if (inp->per_section_minimum_duration) { + memmove(inp->per_section_minimum_duration, + inp->per_section_minimum_duration + 1, + new_nsec * sizeof(double)); + } + (void)old_nsec; + } +} diff --git a/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/output_parameter.h b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/output_parameter.h new file mode 100644 index 0000000..c7ad420 --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/output_parameter.h @@ -0,0 +1,37 @@ +/* + * cruckig - Pure C99 port of the Ruckig trajectory generation library + * Copyright (c) 2025 Yang Yang + * Copyright (c) 2021 Lars Berscheid (original C++ Ruckig) + * + * License: MIT, see the LICENSE file in this directory. + */ +#ifndef CRUCKIG_OUTPUT_PARAMETER_H +#define CRUCKIG_OUTPUT_PARAMETER_H + +#include "cruckig_internal.h" +#include "trajectory.h" +#include "input_parameter.h" + +typedef struct { + size_t degrees_of_freedom; + + CRuckigTrajectory *trajectory; + + double *new_position; + double *new_velocity; + double *new_acceleration; + double *new_jerk; + + double time; + size_t new_section; + bool did_section_change; + bool new_calculation; + bool was_calculation_interrupted; + double calculation_duration; /* microseconds */ +} CRuckigOutputParameter; + +CRuckigOutputParameter* cruckig_output_create(size_t dofs); +void cruckig_output_destroy(CRuckigOutputParameter *out); +void cruckig_output_pass_to_input(const CRuckigOutputParameter *out, CRuckigInputParameter *inp); + +#endif /* CRUCKIG_OUTPUT_PARAMETER_H */ diff --git a/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/position.h b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/position.h new file mode 100644 index 0000000..22ab3a8 --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/position.h @@ -0,0 +1,103 @@ +/* + * cruckig - Pure C99 port of the Ruckig trajectory generation library + * Copyright (c) 2025 Yang Yang + * Copyright (c) 2021 Lars Berscheid (original C++ Ruckig) + * + * License: MIT, see the LICENSE file in this directory. + */ +#ifndef CRUCKIG_POSITION_H +#define CRUCKIG_POSITION_H + +#include "cruckig_internal.h" +#include "profile.h" +#include "block.h" + +/* ---- Third Order Step 1 ---- */ +typedef struct { + double v0, a0, vf, af; + double _vMax, _vMin, _aMax, _aMin, _jMax; + double pd; + double v0_v0, vf_vf; + double a0_a0, a0_p3, a0_p4; + double af_af, af_p3, af_p4; + double jMax_jMax; + CRuckigProfile valid_profiles[6]; +} CRuckigPositionThirdOrderStep1; + +void cruckig_pos3_step1_init(CRuckigPositionThirdOrderStep1 *s, + double p0, double v0, double a0, + double pf, double vf, double af, + double vMax, double vMin, double aMax, double aMin, double jMax); +bool cruckig_pos3_step1_get_profile(CRuckigPositionThirdOrderStep1 *s, + const CRuckigProfile *input, CRuckigBlock *block); + +/* ---- Third Order Step 2 ---- */ +typedef struct { + double v0, a0, tf, vf, af; + double _vMax, _vMin, _aMax, _aMin, _jMax; + double pd; + double tf_tf, tf_p3, tf_p4; + double vd, vd_vd; + double ad, ad_ad; + double v0_v0, vf_vf; + double a0_a0, a0_p3, a0_p4, a0_p5, a0_p6; + double af_af, af_p3, af_p4, af_p5, af_p6; + double jMax_jMax; + double g1, g2; +} CRuckigPositionThirdOrderStep2; + +void cruckig_pos3_step2_init(CRuckigPositionThirdOrderStep2 *s, + double tf, double p0, double v0, double a0, + double pf, double vf, double af, + double vMax, double vMin, double aMax, double aMin, double jMax); +bool cruckig_pos3_step2_get_profile(CRuckigPositionThirdOrderStep2 *s, CRuckigProfile *profile); + +/* ---- Second Order Step 1 ---- */ +typedef struct { + double v0, vf; + double _vMax, _vMin, _aMax, _aMin; + double pd; + CRuckigProfile valid_profiles[4]; +} CRuckigPositionSecondOrderStep1; + +void cruckig_pos2_step1_init(CRuckigPositionSecondOrderStep1 *s, + double p0, double v0, double pf, double vf, + double vMax, double vMin, double aMax, double aMin); +bool cruckig_pos2_step1_get_profile(CRuckigPositionSecondOrderStep1 *s, + const CRuckigProfile *input, CRuckigBlock *block); + +/* ---- Second Order Step 2 ---- */ +typedef struct { + double v0, tf, vf; + double _vMax, _vMin, _aMax, _aMin; + double pd, vd; +} CRuckigPositionSecondOrderStep2; + +void cruckig_pos2_step2_init(CRuckigPositionSecondOrderStep2 *s, + double tf, double p0, double v0, double pf, double vf, + double vMax, double vMin, double aMax, double aMin); +bool cruckig_pos2_step2_get_profile(CRuckigPositionSecondOrderStep2 *s, CRuckigProfile *profile); + +/* ---- First Order Step 1 ---- */ +typedef struct { + double _vMax, _vMin; + double pd; +} CRuckigPositionFirstOrderStep1; + +void cruckig_pos1_step1_init(CRuckigPositionFirstOrderStep1 *s, + double p0, double pf, double vMax, double vMin); +bool cruckig_pos1_step1_get_profile(CRuckigPositionFirstOrderStep1 *s, + const CRuckigProfile *input, CRuckigBlock *block); + +/* ---- First Order Step 2 ---- */ +typedef struct { + double tf; + double _vMax, _vMin; + double pd; +} CRuckigPositionFirstOrderStep2; + +void cruckig_pos1_step2_init(CRuckigPositionFirstOrderStep2 *s, + double tf, double p0, double pf, double vMax, double vMin); +bool cruckig_pos1_step2_get_profile(CRuckigPositionFirstOrderStep2 *s, CRuckigProfile *profile); + +#endif /* CRUCKIG_POSITION_H */ diff --git a/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/position_first_step1.c b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/position_first_step1.c new file mode 100644 index 0000000..9edba78 --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/position_first_step1.c @@ -0,0 +1,41 @@ +/* + * cruckig - Pure C99 port of the Ruckig trajectory generation library + * Copyright (c) 2025 Yang Yang + * Copyright (c) 2021 Lars Berscheid (original C++ Ruckig) + * + * License: MIT, see the LICENSE file in this directory. + */ + +#include "position.h" +#include "block.h" +#include "profile.h" + +void cruckig_pos1_step1_init(CRuckigPositionFirstOrderStep1 *s, + double p0, double pf, double vMax, double vMin) +{ + s->_vMax = vMax; + s->_vMin = vMin; + s->pd = pf - p0; +} + +bool cruckig_pos1_step1_get_profile(CRuckigPositionFirstOrderStep1 *s, + const CRuckigProfile *input, CRuckigBlock *block) +{ + CRuckigProfile *p = &block->p_min; + cruckig_profile_set_boundary_from_profile(p, input); + + const double vf = (s->pd > 0) ? s->_vMax : s->_vMin; + p->t[0] = 0; + p->t[1] = 0; + p->t[2] = 0; + p->t[3] = s->pd / vf; + p->t[4] = 0; + p->t[5] = 0; + p->t[6] = 0; + + if (cruckig_profile_check_for_first_order(p, ControlSignsUDDU, ReachedLimitsVEL, vf)) { + block->t_min = p->t_sum[6] + p->brake.duration + p->accel.duration; + return true; + } + return false; +} diff --git a/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/position_first_step2.c b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/position_first_step2.c new file mode 100644 index 0000000..ff9dd33 --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/position_first_step2.c @@ -0,0 +1,37 @@ +/* + * cruckig - Pure C99 port of the Ruckig trajectory generation library + * Copyright (c) 2025 Yang Yang + * Copyright (c) 2021 Lars Berscheid (original C++ Ruckig) + * + * License: MIT, see the LICENSE file in this directory. + */ + +#include "position.h" +#include "block.h" +#include "profile.h" +#include "roots.h" + +void cruckig_pos1_step2_init(CRuckigPositionFirstOrderStep2 *s, + double tf, double p0, double pf, double vMax, double vMin) +{ + s->tf = tf; + s->_vMax = vMax; + s->_vMin = vMin; + s->pd = pf - p0; +} + +bool cruckig_pos1_step2_get_profile(CRuckigPositionFirstOrderStep2 *s, CRuckigProfile *profile) +{ + const double vf = s->pd / s->tf; + + profile->t[0] = 0; + profile->t[1] = 0; + profile->t[2] = 0; + profile->t[3] = s->tf; + profile->t[4] = 0; + profile->t[5] = 0; + profile->t[6] = 0; + + return cruckig_profile_check_for_first_order_with_timing_full(profile, ControlSignsUDDU, ReachedLimitsNONE, + s->tf, vf, s->_vMax, s->_vMin); +} diff --git a/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/position_second_step1.c b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/position_second_step1.c new file mode 100644 index 0000000..44f9dcb --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/position_second_step1.c @@ -0,0 +1,179 @@ +/* + * cruckig - Pure C99 port of the Ruckig trajectory generation library + * Copyright (c) 2025 Yang Yang + * Copyright (c) 2021 Lars Berscheid (original C++ Ruckig) + * + * License: MIT, see the LICENSE file in this directory. + */ + +#include "position.h" +#include "block.h" +#include "profile.h" + +void cruckig_pos2_step1_init(CRuckigPositionSecondOrderStep1 *s, + double p0, double v0, double pf, double vf, + double vMax, double vMin, double aMax, double aMin) +{ + s->v0 = v0; + s->vf = vf; + s->_vMax = vMax; + s->_vMin = vMin; + s->_aMax = aMax; + s->_aMin = aMin; + s->pd = pf - p0; +} + +static void time_acc0(CRuckigPositionSecondOrderStep1 *s, + CRuckigProfile *valid_profiles, size_t *counter, + double vMax, double vMin, double aMax, double aMin, bool return_after_found) +{ + CRuckigProfile *profile = &valid_profiles[*counter]; + + profile->t[0] = (-s->v0 + vMax) / aMax; + profile->t[1] = (aMin * s->v0 * s->v0 - aMax * s->vf * s->vf) / (2 * aMax * aMin * vMax) + vMax * (aMax - aMin) / (2 * aMax * aMin) + s->pd / vMax; + profile->t[2] = (s->vf - vMax) / aMin; + profile->t[3] = 0; + profile->t[4] = 0; + profile->t[5] = 0; + profile->t[6] = 0; + + if (cruckig_profile_check_for_second_order(profile, ControlSignsUDDU, ReachedLimitsACC0, aMax, aMin, vMax, vMin)) { + ++(*counter); + if (*counter < 4) { + cruckig_profile_set_boundary_from_profile(&valid_profiles[*counter], profile); + } + } + + (void)return_after_found; +} + +static void time_none(CRuckigPositionSecondOrderStep1 *s, + CRuckigProfile *valid_profiles, size_t *counter, + double vMax, double vMin, double aMax, double aMin, bool return_after_found) +{ + double h1 = (aMax * s->vf * s->vf - aMin * s->v0 * s->v0 - 2 * aMax * aMin * s->pd) / (aMax - aMin); + if (h1 >= 0.0) { + h1 = sqrt(h1); + + /* Solution 1 */ + { + CRuckigProfile *profile = &valid_profiles[*counter]; + + profile->t[0] = -(s->v0 + h1) / aMax; + profile->t[1] = 0; + profile->t[2] = (s->vf + h1) / aMin; + profile->t[3] = 0; + profile->t[4] = 0; + profile->t[5] = 0; + profile->t[6] = 0; + + if (cruckig_profile_check_for_second_order(profile, ControlSignsUDDU, ReachedLimitsNONE, aMax, aMin, vMax, vMin)) { + ++(*counter); + if (*counter < 4) { + cruckig_profile_set_boundary_from_profile(&valid_profiles[*counter], profile); + } + if (return_after_found) { + return; + } + } + } + + /* Solution 2 */ + { + CRuckigProfile *profile = &valid_profiles[*counter]; + + profile->t[0] = (-s->v0 + h1) / aMax; + profile->t[1] = 0; + profile->t[2] = (s->vf - h1) / aMin; + profile->t[3] = 0; + profile->t[4] = 0; + profile->t[5] = 0; + profile->t[6] = 0; + + if (cruckig_profile_check_for_second_order(profile, ControlSignsUDDU, ReachedLimitsNONE, aMax, aMin, vMax, vMin)) { + ++(*counter); + if (*counter < 4) { + cruckig_profile_set_boundary_from_profile(&valid_profiles[*counter], profile); + } + } + } + } +} + +static bool time_all_single_step(CRuckigPositionSecondOrderStep1 *s, + CRuckigProfile *profile, double vMax, double vMin) +{ + if (fabs(s->vf - s->v0) > DBL_EPSILON) { + return false; + } + + profile->t[0] = 0; + profile->t[1] = 0; + profile->t[2] = 0; + profile->t[3] = 0; + profile->t[4] = 0; + profile->t[5] = 0; + profile->t[6] = 0; + + if (fabs(s->v0) > DBL_EPSILON) { + profile->t[3] = s->pd / s->v0; + if (cruckig_profile_check_for_second_order(profile, ControlSignsUDDU, ReachedLimitsNONE, 0.0, 0.0, vMax, vMin)) { + return true; + } + } else if (fabs(s->pd) < DBL_EPSILON) { + if (cruckig_profile_check_for_second_order(profile, ControlSignsUDDU, ReachedLimitsNONE, 0.0, 0.0, vMax, vMin)) { + return true; + } + } + + return false; +} + +bool cruckig_pos2_step1_get_profile(CRuckigPositionSecondOrderStep1 *s, + const CRuckigProfile *input, CRuckigBlock *block) +{ + /* Zero-limits special case */ + if (s->_vMax == 0.0 && s->_vMin == 0.0) { + CRuckigProfile *p = &block->p_min; + cruckig_profile_set_boundary_from_profile(p, input); + + if (time_all_single_step(s, p, s->_vMax, s->_vMin)) { + block->t_min = p->t_sum[6] + p->brake.duration + p->accel.duration; + if (fabs(s->v0) > DBL_EPSILON) { + block->a.valid = true; + block->a.left = block->t_min; + block->a.right = INFINITY; + } + return true; + } + return false; + } + + size_t valid_profile_counter = 0; + cruckig_profile_set_boundary_from_profile(&s->valid_profiles[0], input); + + if (fabs(s->vf) < DBL_EPSILON) { + /* There is no blocked interval when vf==0, so return after first found profile */ + const double vMax = (s->pd >= 0) ? s->_vMax : s->_vMin; + const double vMin = (s->pd >= 0) ? s->_vMin : s->_vMax; + const double aMax = (s->pd >= 0) ? s->_aMax : s->_aMin; + const double aMin = (s->pd >= 0) ? s->_aMin : s->_aMax; + + time_none(s, s->valid_profiles, &valid_profile_counter, vMax, vMin, aMax, aMin, true); + if (valid_profile_counter > 0) { goto return_block; } + time_acc0(s, s->valid_profiles, &valid_profile_counter, vMax, vMin, aMax, aMin, true); + if (valid_profile_counter > 0) { goto return_block; } + + time_none(s, s->valid_profiles, &valid_profile_counter, vMin, vMax, aMin, aMax, true); + if (valid_profile_counter > 0) { goto return_block; } + time_acc0(s, s->valid_profiles, &valid_profile_counter, vMin, vMax, aMin, aMax, true); + } else { + time_none(s, s->valid_profiles, &valid_profile_counter, s->_vMax, s->_vMin, s->_aMax, s->_aMin, false); + time_none(s, s->valid_profiles, &valid_profile_counter, s->_vMin, s->_vMax, s->_aMin, s->_aMax, false); + time_acc0(s, s->valid_profiles, &valid_profile_counter, s->_vMax, s->_vMin, s->_aMax, s->_aMin, false); + time_acc0(s, s->valid_profiles, &valid_profile_counter, s->_vMin, s->_vMax, s->_aMin, s->_aMax, false); + } + +return_block: + return cruckig_block_calculate(block, s->valid_profiles, valid_profile_counter, 4); +} diff --git a/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/position_second_step2.c b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/position_second_step2.c new file mode 100644 index 0000000..eda1633 --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/position_second_step2.c @@ -0,0 +1,146 @@ +/* + * cruckig - Pure C99 port of the Ruckig trajectory generation library + * Copyright (c) 2025 Yang Yang + * Copyright (c) 2021 Lars Berscheid (original C++ Ruckig) + * + * License: MIT, see the LICENSE file in this directory. + */ + +#include "position.h" +#include "block.h" +#include "profile.h" +#include "roots.h" + +void cruckig_pos2_step2_init(CRuckigPositionSecondOrderStep2 *s, + double tf, double p0, double v0, double pf, double vf, + double vMax, double vMin, double aMax, double aMin) +{ + s->v0 = v0; + s->tf = tf; + s->vf = vf; + s->_vMax = vMax; + s->_vMin = vMin; + s->_aMax = aMax; + s->_aMin = aMin; + s->pd = pf - p0; + s->vd = vf - v0; +} + +static bool time_acc0(CRuckigPositionSecondOrderStep2 *s, CRuckigProfile *profile, + double vMax, double vMin, double aMax, double aMin) +{ + /* UD Solution 1/2 */ + { + const double h1 = sqrt((2 * aMax * (s->pd - s->tf * s->vf) - 2 * aMin * (s->pd - s->tf * s->v0) + s->vd * s->vd) / (aMax * aMin) + s->tf * s->tf); + + profile->t[0] = (aMax * s->vd - aMax * aMin * (s->tf - h1)) / (aMax * (aMax - aMin)); + profile->t[1] = h1; + profile->t[2] = s->tf - (profile->t[0] + h1); + profile->t[3] = 0; + profile->t[4] = 0; + profile->t[5] = 0; + profile->t[6] = 0; + + if (cruckig_profile_check_for_second_order_with_timing(profile, ControlSignsUDDU, ReachedLimitsACC0, s->tf, aMax, aMin, vMax, vMin)) { + profile->pf = profile->p[7]; + return true; + } + } + + /* UU Solution */ + { + const double h1 = (-s->vd + aMax * s->tf); + + profile->t[0] = -s->vd * s->vd / (2 * aMax * h1) + (s->pd - s->v0 * s->tf) / h1; + profile->t[1] = -s->vd / aMax + s->tf; + profile->t[2] = 0; + profile->t[3] = 0; + profile->t[4] = 0; + profile->t[5] = 0; + profile->t[6] = s->tf - (profile->t[0] + profile->t[1]); + + if (cruckig_profile_check_for_second_order_with_timing(profile, ControlSignsUDDU, ReachedLimitsACC0, s->tf, aMax, aMin, vMax, vMin)) { + profile->pf = profile->p[7]; + return true; + } + } + + /* UU Solution - 2 step */ + { + profile->t[0] = 0; + profile->t[1] = -s->vd / aMax + s->tf; + profile->t[2] = 0; + profile->t[3] = 0; + profile->t[4] = 0; + profile->t[5] = 0; + profile->t[6] = s->vd / aMax; + + if (cruckig_profile_check_for_second_order_with_timing(profile, ControlSignsUDDU, ReachedLimitsACC0, s->tf, aMax, aMin, vMax, vMin)) { + profile->pf = profile->p[7]; + return true; + } + } + + return false; +} + +static bool time_none(CRuckigPositionSecondOrderStep2 *s, CRuckigProfile *profile, + double vMax, double vMin, double aMax, double aMin) +{ + if (fabs(s->v0) < DBL_EPSILON && fabs(s->vf) < DBL_EPSILON && fabs(s->pd) < DBL_EPSILON) { + profile->t[0] = 0; + profile->t[1] = s->tf; + profile->t[2] = 0; + profile->t[3] = 0; + profile->t[4] = 0; + profile->t[5] = 0; + profile->t[6] = 0; + + if (cruckig_profile_check_for_second_order_with_timing(profile, ControlSignsUDDU, ReachedLimitsNONE, s->tf, aMax, aMin, vMax, vMin)) { + profile->pf = profile->p[7]; + return true; + } + } + + /* UD Solution 1/2 */ + { + const double h1 = 2 * (s->vf * s->tf - s->pd); + + profile->t[0] = h1 / s->vd; + profile->t[1] = s->tf - profile->t[0]; + profile->t[2] = 0; + profile->t[3] = 0; + profile->t[4] = 0; + profile->t[5] = 0; + profile->t[6] = 0; + + const double af = s->vd * s->vd / h1; + + if ((aMin - 1e-12 < af) && (af < aMax + 1e-12) && + cruckig_profile_check_for_second_order_with_timing(profile, ControlSignsUDDU, ReachedLimitsNONE, s->tf, af, -af, vMax, vMin)) { + profile->pf = profile->p[7]; + return true; + } + } + + return false; +} + +static bool check_all(CRuckigPositionSecondOrderStep2 *s, CRuckigProfile *profile, + double vMax, double vMin, double aMax, double aMin) +{ + return time_acc0(s, profile, vMax, vMin, aMax, aMin) || + time_none(s, profile, vMax, vMin, aMax, aMin); +} + +bool cruckig_pos2_step2_get_profile(CRuckigPositionSecondOrderStep2 *s, CRuckigProfile *profile) +{ + /* Test all cases to get ones that match */ + if (s->pd > 0) { + return check_all(s, profile, s->_vMax, s->_vMin, s->_aMax, s->_aMin) || + check_all(s, profile, s->_vMin, s->_vMax, s->_aMin, s->_aMax); + } + + return check_all(s, profile, s->_vMin, s->_vMax, s->_aMin, s->_aMax) || + check_all(s, profile, s->_vMax, s->_vMin, s->_aMax, s->_aMin); +} diff --git a/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/position_third_step1.c b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/position_third_step1.c new file mode 100644 index 0000000..b7f1522 --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/position_third_step1.c @@ -0,0 +1,705 @@ +/* + * cruckig - Pure C99 port of the Ruckig trajectory generation library + * Copyright (c) 2025 Yang Yang + * Copyright (c) 2021 Lars Berscheid (original C++ Ruckig) + * + * License: MIT, see the LICENSE file in this directory. + */ + +#include "position.h" +#include "block.h" +#include "profile.h" +#include "roots.h" + +void cruckig_pos3_step1_init(CRuckigPositionThirdOrderStep1 *s, + double p0, double v0, double a0, + double pf, double vf, double af, + double vMax, double vMin, double aMax, double aMin, double jMax) +{ + s->v0 = v0; + s->a0 = a0; + s->vf = vf; + s->af = af; + s->_vMax = vMax; + s->_vMin = vMin; + s->_aMax = aMax; + s->_aMin = aMin; + s->_jMax = jMax; + + s->pd = pf - p0; + + s->v0_v0 = v0 * v0; + s->vf_vf = vf * vf; + + s->a0_a0 = a0 * a0; + s->af_af = af * af; + + s->a0_p3 = a0 * s->a0_a0; + s->a0_p4 = s->a0_a0 * s->a0_a0; + s->af_p3 = af * s->af_af; + s->af_p4 = s->af_af * s->af_af; + + s->jMax_jMax = jMax * jMax; +} + +/* Helper: add_profile equivalent - increment counter, copy boundary to next */ +static inline void add_profile(CRuckigProfile *valid_profiles, size_t *counter, size_t max_profiles) +{ + const size_t prev = *counter; + ++(*counter); + if (*counter < max_profiles) { + cruckig_profile_set_boundary_from_profile(&valid_profiles[*counter], &valid_profiles[prev]); + } +} + +static void time_all_vel(CRuckigPositionThirdOrderStep1 *s, + CRuckigProfile *valid_profiles, size_t *counter, + double vMax, double vMin, double aMax, double aMin, double jMax, + bool return_after_found) +{ + CRuckigProfile *profile = &valid_profiles[*counter]; + const double v0 = s->v0, a0 = s->a0, vf = s->vf, af = s->af; + const double v0_v0 = s->v0_v0, vf_vf = s->vf_vf; + const double a0_a0 = s->a0_a0, af_af = s->af_af; + const double a0_p3 = s->a0_p3, af_p3 = s->af_p3; + const double a0_p4 = s->a0_p4, af_p4 = s->af_p4; + const double jMax_jMax = s->jMax_jMax; + const double pd = s->pd; + + (void)return_after_found; + + /* ACC0_ACC1_VEL */ + profile->t[0] = (-a0 + aMax) / jMax; + profile->t[1] = (a0_a0 / 2 - aMax * aMax - jMax * (v0 - vMax)) / (aMax * jMax); + profile->t[2] = aMax / jMax; + profile->t[3] = (3 * (a0_p4 * aMin - af_p4 * aMax) + 8 * aMax * aMin * (af_p3 - a0_p3 + 3 * jMax * (a0 * v0 - af * vf)) + 6 * a0_a0 * aMin * (aMax * aMax - 2 * jMax * v0) - 6 * af_af * aMax * (aMin * aMin - 2 * jMax * vf) - 12 * jMax * (aMax * aMin * (aMax * (v0 + vMax) - aMin * (vf + vMax) - 2 * jMax * pd) + (aMin - aMax) * jMax * vMax * vMax + jMax * (aMax * vf_vf - aMin * v0_v0))) / (24 * aMax * aMin * jMax_jMax * vMax); + profile->t[4] = -aMin / jMax; + profile->t[5] = -(af_af / 2 - aMin * aMin - jMax * (vf - vMax)) / (aMin * jMax); + profile->t[6] = profile->t[4] + af / jMax; + + if (cruckig_profile_check(profile, ControlSignsUDDU, ReachedLimitsACC0_ACC1_VEL, false, jMax, vMax, vMin, aMax, aMin)) { + add_profile(valid_profiles, counter, 6); + return; + } + + /* ACC1_VEL */ + { + const double t_acc0 = sqrt(a0_a0 / (2 * jMax_jMax) + (vMax - v0) / jMax); + + profile->t[0] = t_acc0 - a0 / jMax; + profile->t[1] = 0; + profile->t[2] = t_acc0; + profile->t[3] = -(3 * af_p4 - 8 * aMin * (af_p3 - a0_p3) - 24 * aMin * jMax * (a0 * v0 - af * vf) + 6 * af_af * (aMin * aMin - 2 * jMax * vf) - 12 * jMax * (2 * aMin * jMax * pd + aMin * aMin * (vf + vMax) + jMax * (vMax * vMax - vf_vf) + aMin * t_acc0 * (a0_a0 - 2 * jMax * (v0 + vMax)))) / (24 * aMin * jMax_jMax * vMax); + + if (cruckig_profile_check(profile, ControlSignsUDDU, ReachedLimitsACC1_VEL, false, jMax, vMax, vMin, aMax, aMin)) { + add_profile(valid_profiles, counter, 6); + return; + } + } + + /* ACC0_VEL */ + { + const double t_acc1 = sqrt(af_af / (2 * jMax_jMax) + (vMax - vf) / jMax); + + profile->t[0] = (-a0 + aMax) / jMax; + profile->t[1] = (a0_a0 / 2 - aMax * aMax - jMax * (v0 - vMax)) / (aMax * jMax); + profile->t[2] = aMax / jMax; + profile->t[3] = (3 * a0_p4 + 8 * aMax * (af_p3 - a0_p3) + 24 * aMax * jMax * (a0 * v0 - af * vf) + 6 * a0_a0 * (aMax * aMax - 2 * jMax * v0) - 12 * jMax * (-2 * aMax * jMax * pd + aMax * aMax * (v0 + vMax) + jMax * (vMax * vMax - v0_v0) + aMax * t_acc1 * (-af_af + 2 * (vf + vMax) * jMax))) / (24 * aMax * jMax_jMax * vMax); + profile->t[4] = t_acc1; + profile->t[5] = 0; + profile->t[6] = t_acc1 + af / jMax; + + if (cruckig_profile_check(profile, ControlSignsUDDU, ReachedLimitsACC0_VEL, false, jMax, vMax, vMin, aMax, aMin)) { + add_profile(valid_profiles, counter, 6); + return; + } + } + + /* VEL */ + { + const double t_acc0 = sqrt(a0_a0 / (2 * jMax_jMax) + (vMax - v0) / jMax); + const double t_acc1 = sqrt(af_af / (2 * jMax_jMax) + (vMax - vf) / jMax); + + /* Solution 3/4 */ + profile->t[0] = t_acc0 - a0 / jMax; + profile->t[1] = 0; + profile->t[2] = t_acc0; + profile->t[3] = (af_p3 - a0_p3) / (3 * jMax_jMax * vMax) + (a0 * v0 - af * vf + (af_af * t_acc1 + a0_a0 * t_acc0) / 2) / (jMax * vMax) - (v0 / vMax + 1.0) * t_acc0 - (vf / vMax + 1.0) * t_acc1 + pd / vMax; + + if (cruckig_profile_check(profile, ControlSignsUDDU, ReachedLimitsVEL, false, jMax, vMax, vMin, aMax, aMin)) { + add_profile(valid_profiles, counter, 6); + } + } +} + +static void time_acc0_acc1(CRuckigPositionThirdOrderStep1 *s, + CRuckigProfile *valid_profiles, size_t *counter, + double vMax, double vMin, double aMax, double aMin, double jMax, + bool return_after_found) +{ + CRuckigProfile *profile = &valid_profiles[*counter]; + const double a0 = s->a0, af = s->af; + const double a0_a0 = s->a0_a0, af_af = s->af_af; + const double a0_p3 = s->a0_p3, af_p3 = s->af_p3; + const double a0_p4 = s->a0_p4, af_p4 = s->af_p4; + const double v0 = s->v0, vf = s->vf; + const double v0_v0 = s->v0_v0, vf_vf = s->vf_vf; + const double jMax_jMax = s->jMax_jMax; + const double pd = s->pd; + + double h1 = (3 * (af_p4 * aMax - a0_p4 * aMin) + aMax * aMin * (8 * (a0_p3 - af_p3) + 3 * aMax * aMin * (aMax - aMin) + 6 * aMin * af_af - 6 * aMax * a0_a0) + 12 * jMax * (aMax * aMin * ((aMax - 2 * a0) * v0 - (aMin - 2 * af) * vf) + aMin * a0_a0 * v0 - aMax * af_af * vf)) / (3 * (aMax - aMin) * jMax_jMax) + 4 * (aMax * vf_vf - aMin * v0_v0 - 2 * aMin * aMax * pd) / (aMax - aMin); + + if (h1 >= 0) { + h1 = sqrt(h1) / 2; + const double h2 = a0_a0 / (2 * aMax * jMax) + (aMin - 2 * aMax) / (2 * jMax) - v0 / aMax; + const double h3 = -af_af / (2 * aMin * jMax) - (aMax - 2 * aMin) / (2 * jMax) + vf / aMin; + + /* UDDU: Solution 2 */ + if (h2 > h1 / aMax && h3 > -h1 / aMin) { + profile->t[0] = (-a0 + aMax) / jMax; + profile->t[1] = h2 - h1 / aMax; + profile->t[2] = aMax / jMax; + profile->t[3] = 0; + profile->t[4] = -aMin / jMax; + profile->t[5] = h3 + h1 / aMin; + profile->t[6] = profile->t[4] + af / jMax; + + if (cruckig_profile_check(profile, ControlSignsUDDU, ReachedLimitsACC0_ACC1, true, jMax, vMax, vMin, aMax, aMin)) { + add_profile(valid_profiles, counter, 6); + if (return_after_found) { + return; + } + } + } + + /* UDDU: Solution 1 */ + profile = &valid_profiles[*counter]; + if (h2 > -h1 / aMax && h3 > h1 / aMin) { + profile->t[0] = (-a0 + aMax) / jMax; + profile->t[1] = h2 + h1 / aMax; + profile->t[2] = aMax / jMax; + profile->t[3] = 0; + profile->t[4] = -aMin / jMax; + profile->t[5] = h3 - h1 / aMin; + profile->t[6] = profile->t[4] + af / jMax; + + if (cruckig_profile_check(profile, ControlSignsUDDU, ReachedLimitsACC0_ACC1, true, jMax, vMax, vMin, aMax, aMin)) { + add_profile(valid_profiles, counter, 6); + } + } + } +} + +static void time_all_none_acc0_acc1(CRuckigPositionThirdOrderStep1 *s, + CRuckigProfile *valid_profiles, size_t *counter, + double vMax, double vMin, double aMax, double aMin, double jMax, + bool return_after_found) +{ + CRuckigProfile *profile = &valid_profiles[*counter]; + const double v0 = s->v0, a0 = s->a0, vf = s->vf, af = s->af; + const double v0_v0 = s->v0_v0, vf_vf = s->vf_vf; + const double a0_a0 = s->a0_a0, af_af = s->af_af; + const double a0_p3 = s->a0_p3, af_p3 = s->af_p3; + const double a0_p4 = s->a0_p4, af_p4 = s->af_p4; + const double jMax_jMax = s->jMax_jMax; + const double pd = s->pd; + + /* NONE UDDU / UDUD Strategy */ + const double h2_none = (a0_a0 - af_af) / (2 * jMax) + (vf - v0); + const double h2_h2 = h2_none * h2_none; + const double t_min_none = (a0 - af) / jMax; + const double t_max_none = (aMax - aMin) / jMax; + + double polynom_none[4]; + polynom_none[0] = 0; + polynom_none[1] = -2 * (a0_a0 + af_af - 2 * jMax * (v0 + vf)) / jMax_jMax; + polynom_none[2] = 4 * (a0_p3 - af_p3 + 3 * jMax * (af * vf - a0 * v0)) / (3 * jMax * jMax_jMax) - 4 * pd / jMax; + polynom_none[3] = -h2_h2 / jMax_jMax; + + /* ACC0 */ + const double h3_acc0 = (a0_a0 - af_af) / (2 * aMax * jMax) + (vf - v0) / aMax; + const double t_min_acc0 = (aMax - af) / jMax; + const double t_max_acc0 = (aMax - aMin) / jMax; + + const double h0_acc0 = 3 * (af_p4 - a0_p4) + 8 * (a0_p3 - af_p3) * aMax + 24 * aMax * jMax * (af * vf - a0 * v0) - 6 * a0_a0 * (aMax * aMax - 2 * jMax * v0) + 6 * af_af * (aMax * aMax - 2 * jMax * vf) + 12 * jMax * (jMax * (vf_vf - v0_v0 - 2 * aMax * pd) - aMax * aMax * (vf - v0)); + const double h2_acc0 = -af_af + aMax * aMax + 2 * jMax * vf; + + double polynom_acc0[4]; + polynom_acc0[0] = -2 * aMax / jMax; + polynom_acc0[1] = h2_acc0 / jMax_jMax; + polynom_acc0[2] = 0; + polynom_acc0[3] = h0_acc0 / (12 * jMax_jMax * jMax_jMax); + + /* ACC1 */ + const double h3_acc1 = -(a0_a0 + af_af) / (2 * jMax * aMin) + aMin / jMax + (vf - v0) / aMin; + const double t_min_acc1 = (aMin - a0) / jMax; + const double t_max_acc1 = (aMax - a0) / jMax; + + const double h0_acc1 = (a0_p4 - af_p4) / 4 + 2 * (af_p3 - a0_p3) * aMin / 3 + (a0_a0 - af_af) * aMin * aMin / 2 + jMax * (af_af * vf + a0_a0 * v0 + 2 * aMin * (jMax * pd - a0 * v0 - af * vf) + aMin * aMin * (v0 + vf) + jMax * (v0_v0 - vf_vf)); + const double h2_acc1 = a0_a0 - a0 * aMin + 2 * jMax * v0; + + double polynom_acc1[4]; + polynom_acc1[0] = 2 * (2 * a0 - aMin) / jMax; + polynom_acc1[1] = (5 * a0_a0 + aMin * (aMin - 6 * a0) + 2 * jMax * v0) / jMax_jMax; + polynom_acc1[2] = 2 * (a0 - aMin) * h2_acc1 / (jMax_jMax * jMax); + polynom_acc1[3] = h0_acc1 / (jMax_jMax * jMax_jMax); + + CRuckigRootSet roots_none = cruckig_roots_solve_quart_monic(polynom_none[0], polynom_none[1], polynom_none[2], polynom_none[3]); + CRuckigRootSet roots_acc0 = cruckig_roots_solve_quart_monic(polynom_acc0[0], polynom_acc0[1], polynom_acc0[2], polynom_acc0[3]); + CRuckigRootSet roots_acc1 = cruckig_roots_solve_quart_monic(polynom_acc1[0], polynom_acc1[1], polynom_acc1[2], polynom_acc1[3]); + + cruckig_root_set_sort(&roots_none); + cruckig_root_set_sort(&roots_acc0); + cruckig_root_set_sort(&roots_acc1); + + for (size_t i = 0; i < roots_none.size; ++i) { + double t = roots_none.data[i]; + if (t < t_min_none || t > t_max_none) { + continue; + } + + /* Single Newton-step (regarding pd) */ + if (t > DBL_EPSILON) { + const double h1 = jMax * t * t; + const double orig = -h2_h2 / (4 * jMax * t) + h2_none * (af / jMax + t) + (4 * a0_p3 + 2 * af_p3 - 6 * a0_a0 * (af + 2 * jMax * t) + 12 * (af - a0) * jMax * v0 + 3 * jMax_jMax * (-4 * pd + (h1 + 8 * v0) * t)) / (12 * jMax_jMax); + const double deriv = h2_none + 2 * v0 - a0_a0 / jMax + h2_h2 / (4 * h1) + (3 * h1) / 4; + + t -= orig / deriv; + } + + const double h0 = h2_none / (2 * jMax * t); + profile = &valid_profiles[*counter]; + profile->t[0] = h0 + t / 2 - a0 / jMax; + profile->t[1] = 0; + profile->t[2] = t; + profile->t[3] = 0; + profile->t[4] = 0; + profile->t[5] = 0; + profile->t[6] = -h0 + t / 2 + af / jMax; + + if (cruckig_profile_check(profile, ControlSignsUDDU, ReachedLimitsNONE, false, jMax, vMax, vMin, aMax, aMin)) { + add_profile(valid_profiles, counter, 6); + if (return_after_found) { + return; + } + } + } + + for (size_t i = 0; i < roots_acc0.size; ++i) { + double t = roots_acc0.data[i]; + if (t < t_min_acc0 || t > t_max_acc0) { + continue; + } + + /* Single Newton step (regarding pd) */ + if (t > DBL_EPSILON) { + const double h1 = jMax * t; + const double orig = h0_acc0 / (12 * jMax_jMax * t) + t * (h2_acc0 + h1 * (h1 - 2 * aMax)); + const double deriv = 2 * (h2_acc0 + h1 * (2 * h1 - 3 * aMax)); + + t -= orig / deriv; + } + + profile = &valid_profiles[*counter]; + profile->t[0] = (-a0 + aMax) / jMax; + profile->t[1] = h3_acc0 - 2 * t + jMax / aMax * t * t; + profile->t[2] = t; + profile->t[3] = 0; + profile->t[4] = 0; + profile->t[5] = 0; + profile->t[6] = (af - aMax) / jMax + t; + + if (cruckig_profile_check(profile, ControlSignsUDDU, ReachedLimitsACC0, false, jMax, vMax, vMin, aMax, aMin)) { + add_profile(valid_profiles, counter, 6); + if (return_after_found) { + return; + } + } + } + + for (size_t i = 0; i < roots_acc1.size; ++i) { + double t = roots_acc1.data[i]; + if (t < t_min_acc1 || t > t_max_acc1) { + continue; + } + + /* Double Newton step (regarding pd) */ + if (t > DBL_EPSILON) { + const double h5 = a0_p3 + 2 * jMax * a0 * v0; + double h1 = jMax * t; + double orig = -(h0_acc1 / 2 + h1 * (h5 + a0 * (aMin - 2 * h1) * (aMin - h1) + a0_a0 * (5 * h1 / 2 - 2 * aMin) + aMin * aMin * h1 / 2 + jMax * (h1 / 2 - aMin) * (h1 * t + 2 * v0))) / jMax; + double deriv = (aMin - a0 - h1) * (h2_acc1 + h1 * (4 * a0 - aMin + 2 * h1)); + { + double correction = orig / deriv; + if (correction > t) correction = t; + t -= correction; + } + + h1 = jMax * t; + orig = -(h0_acc1 / 2 + h1 * (h5 + a0 * (aMin - 2 * h1) * (aMin - h1) + a0_a0 * (5 * h1 / 2 - 2 * aMin) + aMin * aMin * h1 / 2 + jMax * (h1 / 2 - aMin) * (h1 * t + 2 * v0))) / jMax; + + if (fabs(orig) > 1e-9) { + deriv = (aMin - a0 - h1) * (h2_acc1 + h1 * (4 * a0 - aMin + 2 * h1)); + t -= orig / deriv; + + h1 = jMax * t; + orig = -(h0_acc1 / 2 + h1 * (h5 + a0 * (aMin - 2 * h1) * (aMin - h1) + a0_a0 * (5 * h1 / 2 - 2 * aMin) + aMin * aMin * h1 / 2 + jMax * (h1 / 2 - aMin) * (h1 * t + 2 * v0))) / jMax; + + if (fabs(orig) > 1e-9) { + deriv = (aMin - a0 - h1) * (h2_acc1 + h1 * (4 * a0 - aMin + 2 * h1)); + t -= orig / deriv; + } + } + } + + profile = &valid_profiles[*counter]; + profile->t[0] = t; + profile->t[1] = 0; + profile->t[2] = (a0 - aMin) / jMax + t; + profile->t[3] = 0; + profile->t[4] = 0; + profile->t[5] = h3_acc1 - (2 * a0 + jMax * t) * t / aMin; + profile->t[6] = (af - aMin) / jMax; + + if (cruckig_profile_check(profile, ControlSignsUDDU, ReachedLimitsACC1, true, jMax, vMax, vMin, aMax, aMin)) { + add_profile(valid_profiles, counter, 6); + if (return_after_found) { + return; + } + } + } +} + +static void time_acc1_vel_two_step(CRuckigPositionThirdOrderStep1 *s, + CRuckigProfile *valid_profiles, size_t *counter, + double vMax, double vMin, double aMax, double aMin, double jMax) +{ + CRuckigProfile *profile = &valid_profiles[*counter]; + const double v0 = s->v0, a0 = s->a0, vf = s->vf, af = s->af; + const double vf_vf = s->vf_vf; + const double a0_a0 = s->a0_a0, af_af = s->af_af; + const double a0_p3 = s->a0_p3, af_p3 = s->af_p3, af_p4 = s->af_p4; + const double jMax_jMax = s->jMax_jMax; + const double pd = s->pd; + + profile->t[0] = 0; + profile->t[1] = 0; + profile->t[2] = a0 / jMax; + profile->t[3] = -(3 * af_p4 - 8 * aMin * (af_p3 - a0_p3) - 24 * aMin * jMax * (a0 * v0 - af * vf) + 6 * af_af * (aMin * aMin - 2 * jMax * vf) - 12 * jMax * (2 * aMin * jMax * pd + aMin * aMin * (vf + vMax) + jMax * (vMax * vMax - vf_vf) + aMin * a0 * (a0_a0 - 2 * jMax * (v0 + vMax)) / jMax)) / (24 * aMin * jMax_jMax * vMax); + profile->t[4] = -aMin / jMax; + profile->t[5] = -(af_af / 2 - aMin * aMin + jMax * (vMax - vf)) / (aMin * jMax); + profile->t[6] = profile->t[4] + af / jMax; + + if (cruckig_profile_check(profile, ControlSignsUDDU, ReachedLimitsACC1_VEL, false, jMax, vMax, vMin, aMax, aMin)) { + add_profile(valid_profiles, counter, 6); + } +} + +static void time_acc0_two_step(CRuckigPositionThirdOrderStep1 *s, + CRuckigProfile *valid_profiles, size_t *counter, + double vMax, double vMin, double aMax, double aMin, double jMax) +{ + CRuckigProfile *profile = &valid_profiles[*counter]; + const double v0 = s->v0, a0 = s->a0, vf = s->vf, af = s->af; + const double v0_v0 = s->v0_v0, vf_vf = s->vf_vf; + const double a0_a0 = s->a0_a0, af_af = s->af_af; + const double a0_p3 = s->a0_p3, af_p3 = s->af_p3; + const double a0_p4 = s->a0_p4, af_p4 = s->af_p4; + const double jMax_jMax = s->jMax_jMax; + const double pd = s->pd; + + /* Two step */ + { + profile->t[0] = 0; + profile->t[1] = (af_af - a0_a0 + 2 * jMax * (vf - v0)) / (2 * a0 * jMax); + profile->t[2] = (a0 - af) / jMax; + profile->t[3] = 0; + profile->t[4] = 0; + profile->t[5] = 0; + profile->t[6] = 0; + + if (cruckig_profile_check(profile, ControlSignsUDDU, ReachedLimitsACC0, false, jMax, vMax, vMin, aMax, aMin)) { + add_profile(valid_profiles, counter, 6); + return; + } + } + + /* Three step - Removed pf */ + { + profile = &valid_profiles[*counter]; + profile->t[0] = (-a0 + aMax) / jMax; + profile->t[1] = (a0_a0 + af_af - 2 * aMax * aMax + 2 * jMax * (vf - v0)) / (2 * aMax * jMax); + profile->t[2] = (-af + aMax) / jMax; + profile->t[3] = 0; + profile->t[4] = 0; + profile->t[5] = 0; + profile->t[6] = 0; + + if (cruckig_profile_check(profile, ControlSignsUDDU, ReachedLimitsACC0, false, jMax, vMax, vMin, aMax, aMin)) { + add_profile(valid_profiles, counter, 6); + return; + } + } + + /* Three step - Removed aMax */ + { + profile = &valid_profiles[*counter]; + const double h0 = 3 * (af_af - a0_a0 + 2 * jMax * (v0 + vf)); + const double h2 = a0_p3 + 2 * af_p3 + 6 * jMax_jMax * pd + 6 * (af - a0) * jMax * vf - 3 * a0 * af_af; + const double h1_sq = 2 * (2 * h2 * h2 + h0 * (a0_p4 - 6 * a0_a0 * (af_af + 2 * jMax * vf) + 8 * a0 * (af_p3 + 3 * jMax_jMax * pd + 3 * af * jMax * vf) - 3 * (af_p4 + 4 * af_af * jMax * vf + 4 * jMax_jMax * (vf_vf - v0_v0)))); + const double h1 = sqrt(h1_sq) * fabs(jMax) / jMax; + profile->t[0] = (4 * af_p3 + 2 * a0_p3 - 6 * a0 * af_af + 12 * jMax_jMax * pd + 12 * (af - a0) * jMax * vf + h1) / (2 * jMax * h0); + profile->t[1] = -h1 / (jMax * h0); + profile->t[2] = (-4 * a0_p3 - 2 * af_p3 + 6 * a0_a0 * af + 12 * jMax_jMax * pd - 12 * (af - a0) * jMax * v0 + h1) / (2 * jMax * h0); + profile->t[3] = 0; + profile->t[4] = 0; + profile->t[5] = 0; + profile->t[6] = 0; + + if (cruckig_profile_check(profile, ControlSignsUDDU, ReachedLimitsACC0, false, jMax, vMax, vMin, aMax, aMin)) { + add_profile(valid_profiles, counter, 6); + return; + } + } + + /* Three step - t=(aMax - aMin)/jMax */ + { + profile = &valid_profiles[*counter]; + const double t = (aMax - aMin) / jMax; + + profile->t[0] = (-a0 + aMax) / jMax; + profile->t[1] = (a0_a0 - af_af) / (2 * aMax * jMax) + (vf - v0 + jMax * t * t) / aMax - 2 * t; + profile->t[2] = t; + profile->t[3] = 0; + profile->t[4] = 0; + profile->t[5] = 0; + profile->t[6] = (af - aMin) / jMax; + + if (cruckig_profile_check(profile, ControlSignsUDDU, ReachedLimitsACC0, false, jMax, vMax, vMin, aMax, aMin)) { + add_profile(valid_profiles, counter, 6); + return; + } + } +} + +static void time_vel_two_step(CRuckigPositionThirdOrderStep1 *s, + CRuckigProfile *valid_profiles, size_t *counter, + double vMax, double vMin, double aMax, double aMin, double jMax) +{ + CRuckigProfile *profile; + const double v0 = s->v0, a0 = s->a0, vf = s->vf, af = s->af; + const double af_af = s->af_af; + const double a0_p3 = s->a0_p3, af_p3 = s->af_p3; + const double jMax_jMax = s->jMax_jMax; + const double pd = s->pd; + + const double h1 = sqrt(af_af / (2 * jMax_jMax) + (vMax - vf) / jMax); + + /* Four step - Solution 3/4 */ + { + profile = &valid_profiles[*counter]; + profile->t[0] = -a0 / jMax; + profile->t[1] = 0; + profile->t[2] = 0; + profile->t[3] = (af_p3 - a0_p3) / (3 * jMax_jMax * vMax) + (a0 * v0 - af * vf + (af_af * h1) / 2) / (jMax * vMax) - (vf / vMax + 1.0) * h1 + pd / vMax; + profile->t[4] = h1; + profile->t[5] = 0; + profile->t[6] = h1 + af / jMax; + + if (cruckig_profile_check(profile, ControlSignsUDDU, ReachedLimitsVEL, false, jMax, vMax, vMin, aMax, aMin)) { + add_profile(valid_profiles, counter, 6); + return; + } + } + + /* Four step */ + { + profile = &valid_profiles[*counter]; + profile->t[0] = 0; + profile->t[1] = 0; + profile->t[2] = a0 / jMax; + profile->t[3] = (af_p3 - a0_p3) / (3 * jMax_jMax * vMax) + (a0 * v0 - af * vf + (af_af * h1 + a0_p3 / jMax) / 2) / (jMax * vMax) - (v0 / vMax + 1.0) * a0 / jMax - (vf / vMax + 1.0) * h1 + pd / vMax; + profile->t[4] = h1; + profile->t[5] = 0; + profile->t[6] = h1 + af / jMax; + + if (cruckig_profile_check(profile, ControlSignsUDDU, ReachedLimitsVEL, false, jMax, vMax, vMin, aMax, aMin)) { + add_profile(valid_profiles, counter, 6); + return; + } + } +} + +static void time_none_two_step(CRuckigPositionThirdOrderStep1 *s, + CRuckigProfile *valid_profiles, size_t *counter, + double vMax, double vMin, double aMax, double aMin, double jMax) +{ + CRuckigProfile *profile; + const double v0 = s->v0, a0 = s->a0, vf = s->vf, af = s->af; + const double a0_a0 = s->a0_a0, af_af = s->af_af; + + /* Two step */ + { + profile = &valid_profiles[*counter]; + const double h0 = sqrt((a0_a0 + af_af) / 2 + jMax * (vf - v0)) * fabs(jMax) / jMax; + profile->t[0] = (h0 - a0) / jMax; + profile->t[1] = 0; + profile->t[2] = (h0 - af) / jMax; + profile->t[3] = 0; + profile->t[4] = 0; + profile->t[5] = 0; + profile->t[6] = 0; + + if (cruckig_profile_check(profile, ControlSignsUDDU, ReachedLimitsNONE, false, jMax, vMax, vMin, aMax, aMin)) { + add_profile(valid_profiles, counter, 6); + return; + } + } + + /* Single step */ + { + profile = &valid_profiles[*counter]; + profile->t[0] = (af - a0) / jMax; + profile->t[1] = 0; + profile->t[2] = 0; + profile->t[3] = 0; + profile->t[4] = 0; + profile->t[5] = 0; + profile->t[6] = 0; + + if (cruckig_profile_check(profile, ControlSignsUDDU, ReachedLimitsNONE, false, jMax, vMax, vMin, aMax, aMin)) { + add_profile(valid_profiles, counter, 6); + return; + } + } +} + +static bool time_all_single_step(CRuckigPositionThirdOrderStep1 *s, + CRuckigProfile *profile, double vMax, double vMin, double aMax, double aMin) +{ + const double v0 = s->v0, a0 = s->a0, af = s->af; + const double v0_v0 = s->v0_v0; + const double pd = s->pd; + + if (fabs(af - a0) > DBL_EPSILON) { + return false; + } + + profile->t[0] = 0; + profile->t[1] = 0; + profile->t[2] = 0; + profile->t[3] = 0; + profile->t[4] = 0; + profile->t[5] = 0; + profile->t[6] = 0; + + if (fabs(a0) > DBL_EPSILON) { + const double q = sqrt(2 * a0 * pd + v0_v0); + + /* Solution 1 */ + profile->t[3] = (-v0 + q) / a0; + if (profile->t[3] >= 0.0 && cruckig_profile_check(profile, ControlSignsUDDU, ReachedLimitsNONE, false, 0.0, vMax, vMin, aMax, aMin)) { + return true; + } + + /* Solution 2 */ + profile->t[3] = -(v0 + q) / a0; + if (profile->t[3] >= 0.0 && cruckig_profile_check(profile, ControlSignsUDDU, ReachedLimitsNONE, false, 0.0, vMax, vMin, aMax, aMin)) { + return true; + } + + } else if (fabs(v0) > DBL_EPSILON) { + profile->t[3] = pd / v0; + if (cruckig_profile_check(profile, ControlSignsUDDU, ReachedLimitsNONE, false, 0.0, vMax, vMin, aMax, aMin)) { + return true; + } + + } else if (fabs(pd) < DBL_EPSILON) { + if (cruckig_profile_check(profile, ControlSignsUDDU, ReachedLimitsNONE, false, 0.0, vMax, vMin, aMax, aMin)) { + return true; + } + } + + return false; +} + +CRUCKIG_HOT +bool cruckig_pos3_step1_get_profile(CRuckigPositionThirdOrderStep1 *s, + const CRuckigProfile *input, CRuckigBlock *block) +{ + /* Zero-limits special case */ + if (s->_jMax == 0.0 || s->_aMax == 0.0 || s->_aMin == 0.0) { + CRuckigProfile *p = &block->p_min; + cruckig_profile_set_boundary_from_profile(p, input); + + if (time_all_single_step(s, p, s->_vMax, s->_vMin, s->_aMax, s->_aMin)) { + block->t_min = p->t_sum[6] + p->brake.duration + p->accel.duration; + if (fabs(s->v0) > DBL_EPSILON || fabs(s->a0) > DBL_EPSILON) { + block->a.valid = true; + block->a.left = block->t_min; + block->a.right = INFINITY; + } + return true; + } + return false; + } + + size_t valid_profile_counter = 0; + cruckig_profile_set_boundary_from_profile(&s->valid_profiles[0], input); + + if (fabs(s->vf) < DBL_EPSILON && fabs(s->af) < DBL_EPSILON) { + const double vMax = (s->pd >= 0) ? s->_vMax : s->_vMin; + const double vMin = (s->pd >= 0) ? s->_vMin : s->_vMax; + const double aMax = (s->pd >= 0) ? s->_aMax : s->_aMin; + const double aMin = (s->pd >= 0) ? s->_aMin : s->_aMax; + const double jMax = (s->pd >= 0) ? s->_jMax : -s->_jMax; + + if (fabs(s->v0) < DBL_EPSILON && fabs(s->a0) < DBL_EPSILON && fabs(s->pd) < DBL_EPSILON) { + time_all_none_acc0_acc1(s, s->valid_profiles, &valid_profile_counter, vMax, vMin, aMax, aMin, jMax, true); + + } else { + /* There is no blocked interval when vf==0 && af==0, so return after first found profile */ + time_all_vel(s, s->valid_profiles, &valid_profile_counter, vMax, vMin, aMax, aMin, jMax, true); + if (valid_profile_counter > 0) { goto return_block; } + time_all_none_acc0_acc1(s, s->valid_profiles, &valid_profile_counter, vMax, vMin, aMax, aMin, jMax, true); + if (valid_profile_counter > 0) { goto return_block; } + time_acc0_acc1(s, s->valid_profiles, &valid_profile_counter, vMax, vMin, aMax, aMin, jMax, true); + if (valid_profile_counter > 0) { goto return_block; } + + time_all_vel(s, s->valid_profiles, &valid_profile_counter, vMin, vMax, aMin, aMax, -jMax, true); + if (valid_profile_counter > 0) { goto return_block; } + time_all_none_acc0_acc1(s, s->valid_profiles, &valid_profile_counter, vMin, vMax, aMin, aMax, -jMax, true); + if (valid_profile_counter > 0) { goto return_block; } + time_acc0_acc1(s, s->valid_profiles, &valid_profile_counter, vMin, vMax, aMin, aMax, -jMax, true); + } + + } else { + time_all_none_acc0_acc1(s, s->valid_profiles, &valid_profile_counter, s->_vMax, s->_vMin, s->_aMax, s->_aMin, s->_jMax, false); + time_all_none_acc0_acc1(s, s->valid_profiles, &valid_profile_counter, s->_vMin, s->_vMax, s->_aMin, s->_aMax, -s->_jMax, false); + time_acc0_acc1(s, s->valid_profiles, &valid_profile_counter, s->_vMax, s->_vMin, s->_aMax, s->_aMin, s->_jMax, false); + time_acc0_acc1(s, s->valid_profiles, &valid_profile_counter, s->_vMin, s->_vMax, s->_aMin, s->_aMax, -s->_jMax, false); + time_all_vel(s, s->valid_profiles, &valid_profile_counter, s->_vMax, s->_vMin, s->_aMax, s->_aMin, s->_jMax, false); + time_all_vel(s, s->valid_profiles, &valid_profile_counter, s->_vMin, s->_vMax, s->_aMin, s->_aMax, -s->_jMax, false); + } + + if (valid_profile_counter == 0) { + time_none_two_step(s, s->valid_profiles, &valid_profile_counter, s->_vMax, s->_vMin, s->_aMax, s->_aMin, s->_jMax); + if (valid_profile_counter > 0) { goto return_block; } + time_none_two_step(s, s->valid_profiles, &valid_profile_counter, s->_vMin, s->_vMax, s->_aMin, s->_aMax, -s->_jMax); + if (valid_profile_counter > 0) { goto return_block; } + time_acc0_two_step(s, s->valid_profiles, &valid_profile_counter, s->_vMax, s->_vMin, s->_aMax, s->_aMin, s->_jMax); + if (valid_profile_counter > 0) { goto return_block; } + time_acc0_two_step(s, s->valid_profiles, &valid_profile_counter, s->_vMin, s->_vMax, s->_aMin, s->_aMax, -s->_jMax); + if (valid_profile_counter > 0) { goto return_block; } + time_vel_two_step(s, s->valid_profiles, &valid_profile_counter, s->_vMax, s->_vMin, s->_aMax, s->_aMin, s->_jMax); + if (valid_profile_counter > 0) { goto return_block; } + time_vel_two_step(s, s->valid_profiles, &valid_profile_counter, s->_vMin, s->_vMax, s->_aMin, s->_aMax, -s->_jMax); + if (valid_profile_counter > 0) { goto return_block; } + time_acc1_vel_two_step(s, s->valid_profiles, &valid_profile_counter, s->_vMax, s->_vMin, s->_aMax, s->_aMin, s->_jMax); + if (valid_profile_counter > 0) { goto return_block; } + time_acc1_vel_two_step(s, s->valid_profiles, &valid_profile_counter, s->_vMin, s->_vMax, s->_aMin, s->_aMax, -s->_jMax); + } + +return_block: + return cruckig_block_calculate(block, s->valid_profiles, valid_profile_counter, 6); +} diff --git a/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/position_third_step2.c b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/position_third_step2.c new file mode 100644 index 0000000..7cbd834 --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/position_third_step2.c @@ -0,0 +1,1370 @@ +/* + * cruckig - Pure C99 port of the Ruckig trajectory generation library + * Copyright (c) 2025 Yang Yang + * Copyright (c) 2021 Lars Berscheid (original C++ Ruckig) + * + * License: MIT, see the LICENSE file in this directory. + */ + +#include "position.h" +#include "block.h" +#include "profile.h" +#include "roots.h" +#include "utils.h" + +#define ROOTS_TOLERANCE 1e-14 + +/* Helper: compute monic derivative of a monic polynomial. + Input: coeffs[0..n-1] where coeffs[0] == 1.0 (monic). + Output: deriv[0..n-2] where deriv[0] = 1.0 (monic). + For i in 1..n-2: deriv[i] = (n-1-i) * coeffs[i] / (n-1) +*/ +static void poly_monic_derivative(const double *coeffs, size_t n, double *deriv) +{ + deriv[0] = 1.0; + for (size_t i = 1; i < n - 1; ++i) { + deriv[i] = (double)(n - 1 - i) * coeffs[i] / (double)(n - 1); + } +} + +void cruckig_pos3_step2_init(CRuckigPositionThirdOrderStep2 *s, + double tf, double p0, double v0, double a0, + double pf, double vf, double af, + double vMax, double vMin, double aMax, double aMin, double jMax) +{ + s->v0 = v0; + s->a0 = a0; + s->tf = tf; + s->vf = vf; + s->af = af; + s->_vMax = vMax; + s->_vMin = vMin; + s->_aMax = aMax; + s->_aMin = aMin; + s->_jMax = jMax; + + s->pd = pf - p0; + s->tf_tf = tf * tf; + s->tf_p3 = s->tf_tf * tf; + s->tf_p4 = s->tf_tf * s->tf_tf; + + s->vd = vf - v0; + s->vd_vd = s->vd * s->vd; + s->v0_v0 = v0 * v0; + s->vf_vf = vf * vf; + + s->ad = af - a0; + s->ad_ad = s->ad * s->ad; + s->a0_a0 = a0 * a0; + s->af_af = af * af; + + s->a0_p3 = a0 * s->a0_a0; + s->a0_p4 = s->a0_a0 * s->a0_a0; + s->a0_p5 = s->a0_p3 * s->a0_a0; + s->a0_p6 = s->a0_p4 * s->a0_a0; + s->af_p3 = af * s->af_af; + s->af_p4 = s->af_af * s->af_af; + s->af_p5 = s->af_p3 * s->af_af; + s->af_p6 = s->af_p4 * s->af_af; + + s->jMax_jMax = jMax * jMax; + + s->g1 = -s->pd + tf * v0; + s->g2 = -2 * s->pd + tf * (v0 + vf); +} + +static bool time_acc0_acc1_vel(CRuckigPositionThirdOrderStep2 *s, CRuckigProfile *profile, + double vMax, double vMin, double aMax, double aMin, double jMax) +{ + const double a0 = s->a0, vf = s->vf, af = s->af; + const double tf = s->tf; + const double pd = s->pd; + const double vd = s->vd, vd_vd = s->vd_vd; + const double ad = s->ad; + const double a0_a0 = s->a0_a0, af_af = s->af_af; + const double a0_p3 = s->a0_p3, af_p3 = s->af_p3; + const double a0_p4 = s->a0_p4, af_p4 = s->af_p4; + const double jMax_jMax = s->jMax_jMax; + const double g1 = s->g1; + + /* Profile UDDU, Solution 1 */ + if ((2 * (aMax - aMin) + ad) / jMax < tf) { + const double h1 = sqrt((a0_p4 + af_p4 - 4 * a0_p3 * (2 * aMax + aMin) / 3 - 4 * af_p3 * (aMax + 2 * aMin) / 3 + 2 * (a0_a0 - af_af) * aMax * aMax + (4 * a0 * aMax - 2 * a0_a0) * (af_af - 2 * af * aMin + (aMin - aMax) * aMin + 2 * jMax * (aMin * tf - vd)) + 2 * af_af * (aMin * aMin + 2 * jMax * (aMax * tf - vd)) + 4 * jMax * (2 * aMin * (af * vd + jMax * g1) + (aMax * aMax - aMin * aMin) * vd + jMax * vd_vd) + 8 * aMax * jMax_jMax * (pd - tf * vf)) / (aMax * aMin) + 4 * af_af + 2 * a0_a0 + (4 * af + aMax - aMin) * (aMax - aMin) + 4 * jMax * (aMin - aMax + jMax * tf - 2 * af) * tf) * fabs(jMax) / jMax; + + profile->t[0] = (-a0 + aMax) / jMax; + profile->t[1] = (-(af_af - a0_a0 + 2 * aMax * aMax + aMin * (aMin - 2 * ad - 3 * aMax) + 2 * jMax * (aMin * tf - vd)) + aMin * h1) / (2 * (aMax - aMin) * jMax); + profile->t[2] = aMax / jMax; + profile->t[3] = (aMin - aMax + h1) / (2 * jMax); + profile->t[4] = -aMin / jMax; + profile->t[5] = tf - (profile->t[0] + profile->t[1] + profile->t[2] + profile->t[3] + 2 * profile->t[4] + af / jMax); + profile->t[6] = profile->t[4] + af / jMax; + + if (cruckig_profile_check_with_timing(profile, ControlSignsUDDU, ReachedLimitsACC0_ACC1_VEL, tf, jMax, vMax, vMin, aMax, aMin)) { + return true; + } + } + + /* Profile UDUD */ + if ((-a0 + 4 * aMax - af) / jMax < tf) { + profile->t[0] = (-a0 + aMax) / jMax; + profile->t[1] = (3 * (a0_p4 + af_p4) - 4 * (a0_p3 + af_p3) * aMax - 4 * af_p3 * aMax + 24 * (a0 + af) * aMax * aMax * aMax - 6 * (af_af + a0_a0) * (aMax * aMax - 2 * jMax * vd) + 6 * a0_a0 * (af_af - 2 * af * aMax - 2 * aMax * jMax * tf) - 12 * aMax * aMax * (2 * aMax * aMax - 2 * aMax * jMax * tf + jMax * vd) - 24 * af * aMax * jMax * vd + 12 * jMax_jMax * (2 * aMax * g1 + vd_vd)) / (12 * aMax * jMax * (a0_a0 + af_af - 2 * (a0 + af) * aMax + 2 * (aMax * aMax - aMax * jMax * tf + jMax * vd))); + profile->t[2] = aMax / jMax; + profile->t[3] = (-a0_a0 - af_af + 2 * aMax * (a0 + af - 2 * aMax) - 2 * jMax * vd) / (2 * aMax * jMax) + tf; + profile->t[4] = profile->t[2]; + profile->t[5] = tf - (profile->t[0] + profile->t[1] + profile->t[2] + profile->t[3] + 2 * profile->t[4] - af / jMax); + profile->t[6] = profile->t[4] - af / jMax; + + if (cruckig_profile_check_with_timing(profile, ControlSignsUDUD, ReachedLimitsACC0_ACC1_VEL, tf, jMax, vMax, vMin, aMax, aMin)) { + return true; + } + } + + return false; +} + +static bool time_acc1_vel(CRuckigPositionThirdOrderStep2 *s, CRuckigProfile *profile, + double vMax, double vMin, double aMax, double aMin, double jMax) +{ + const double v0 = s->v0, a0 = s->a0, af = s->af; + const double tf = s->tf; + const double pd = s->pd; + const double vd = s->vd, vd_vd = s->vd_vd; + const double ad = s->ad; + const double a0_a0 = s->a0_a0, af_af = s->af_af; + const double a0_p3 = s->a0_p3, af_p3 = s->af_p3; + const double a0_p4 = s->a0_p4, af_p4 = s->af_p4; + const double jMax_jMax = s->jMax_jMax; + const double g1 = s->g1; + + /* Profile UDDU */ + { + const double ph1 = a0_a0 + af_af - aMin * (a0 + 2 * af - aMin) - 2 * jMax * (vd - aMin * tf); + const double ph2 = 2 * aMin * (jMax * g1 + af * vd) - aMin * aMin * vd + jMax * vd_vd; + const double ph3 = af_af + aMin * (aMin - 2 * af) - 2 * jMax * (vd - aMin * tf); + + double polynom[4]; + polynom[0] = (2 * (2 * a0 - aMin)) / jMax; + polynom[1] = (4 * a0_a0 + ph1 - 3 * a0 * aMin) / jMax_jMax; + polynom[2] = (2 * a0 * ph1) / (jMax_jMax * jMax); + polynom[3] = (3 * (a0_p4 + af_p4) - 4 * (a0_p3 + 2 * af_p3) * aMin + 6 * af_af * (aMin * aMin - 2 * jMax * vd) + 12 * jMax * ph2 + 6 * a0_a0 * ph3) / (12 * jMax_jMax * jMax_jMax); + + const double t_min = -a0 / jMax; + double t_max_a = (tf + 2 * aMin / jMax - (a0 + af) / jMax) / 2; + double t_max_b = (aMax - a0) / jMax; + const double t_max = (t_max_a < t_max_b) ? t_max_a : t_max_b; + + CRuckigRootSet roots = cruckig_roots_solve_quart_monic(polynom[0], polynom[1], polynom[2], polynom[3]); + cruckig_root_set_sort(&roots); + for (size_t i = 0; i < roots.size; ++i) { + double t = roots.data[i]; + if (t < t_min || t > t_max) { + continue; + } + + /* Single Newton step (regarding pd) */ + if (fabs(a0 + jMax * t) > 16 * DBL_EPSILON) { + const double h0 = jMax * t * t; + const double orig = -pd + (3 * (a0_p4 + af_p4) - 8 * af_p3 * aMin - 4 * a0_p3 * aMin + 6 * af_af * (aMin * aMin + 2 * jMax * (h0 - vd)) + 6 * a0_a0 * (af_af - 2 * af * aMin + aMin * aMin + 2 * aMin * jMax * (-2 * t + tf) + 2 * jMax * (5 * h0 - vd)) + 24 * a0 * jMax * t * (a0_a0 + af_af - 2 * af * aMin + aMin * aMin + 2 * jMax * (aMin * (-t + tf) + h0 - vd)) - 24 * af * aMin * jMax * (h0 - vd) + 12 * jMax * (aMin * aMin * (h0 - vd) + jMax * (h0 - vd) * (h0 - vd))) / (24 * aMin * jMax_jMax) + h0 * (tf - t) + tf * v0; + const double deriv = (a0 + jMax * t) * ((a0_a0 + af_af) / (aMin * jMax) + (aMin - a0 - 2 * af) / jMax + (4 * a0 * t + 2 * h0 - 2 * vd) / aMin + 2 * tf - 3 * t); + + t -= orig / deriv; + } + + { + const double h1 = -((a0_a0 + af_af) / 2 + jMax * (-vd + 2 * a0 * t + jMax * t * t)) / aMin; + + profile->t[0] = t; + profile->t[1] = 0; + profile->t[2] = a0 / jMax + t; + profile->t[3] = tf - (h1 - aMin + a0 + af) / jMax - 2 * t; + profile->t[4] = -aMin / jMax; + profile->t[5] = (h1 + aMin) / jMax; + profile->t[6] = profile->t[4] + af / jMax; + + if (cruckig_profile_check_with_timing(profile, ControlSignsUDDU, ReachedLimitsACC1_VEL, tf, jMax, vMax, vMin, aMax, aMin)) { + return true; + } + } + } + } + + /* Profile UDUD */ + { + const double ph1 = a0_a0 - af_af + (2 * af - a0) * aMax - aMax * aMax - 2 * jMax * (vd - aMax * tf); + const double ph2 = aMax * aMax + 2 * jMax * vd; + const double ph3 = af_af + ph2 - 2 * aMax * (af + jMax * tf); + const double ph4 = 2 * aMax * jMax * g1 + aMax * aMax * vd + jMax * vd_vd; + + double polynom[4]; + polynom[0] = (4 * a0 - 2 * aMax) / jMax; + polynom[1] = (4 * a0_a0 - 3 * a0 * aMax + ph1) / jMax_jMax; + polynom[2] = (2 * a0 * ph1) / (jMax_jMax * jMax); + polynom[3] = (3 * (a0_p4 + af_p4) - 4 * (a0_p3 + 2 * af_p3) * aMax - 24 * af * aMax * jMax * vd + 12 * jMax * ph4 - 6 * a0_a0 * ph3 + 6 * af_af * ph2) / (12 * jMax_jMax * jMax_jMax); + + const double t_min = -a0 / jMax; + double t_max_a = (tf + ad / jMax - 2 * aMax / jMax) / 2; + double t_max_b = (aMax - a0) / jMax; + const double t_max = (t_max_a < t_max_b) ? t_max_a : t_max_b; + + CRuckigRootSet roots = cruckig_roots_solve_quart_monic(polynom[0], polynom[1], polynom[2], polynom[3]); + cruckig_root_set_sort(&roots); + for (size_t i = 0; i < roots.size; ++i) { + double t = roots.data[i]; + if (t > t_max || t < t_min) { + continue; + } + + { + const double h1 = ((a0_a0 - af_af) / 2 + jMax_jMax * t * t - jMax * (vd - 2 * a0 * t)) / aMax; + + profile->t[0] = t; + profile->t[1] = 0; + profile->t[2] = t + a0 / jMax; + profile->t[3] = tf + (h1 + ad - aMax) / jMax - 2 * t; + profile->t[4] = aMax / jMax; + profile->t[5] = -(h1 + aMax) / jMax; + profile->t[6] = profile->t[4] - af / jMax; + + if (cruckig_profile_check_with_timing(profile, ControlSignsUDUD, ReachedLimitsACC1_VEL, tf, jMax, vMax, vMin, aMax, aMin)) { + return true; + } + } + } + } + + return false; +} + +static bool time_acc0_vel(CRuckigPositionThirdOrderStep2 *s, CRuckigProfile *profile, + double vMax, double vMin, double aMax, double aMin, double jMax) +{ + const double a0 = s->a0, vf = s->vf, af = s->af; + const double tf = s->tf; + const double pd = s->pd; + const double vd = s->vd, vd_vd = s->vd_vd; + const double ad = s->ad; + const double a0_a0 = s->a0_a0, af_af = s->af_af; + const double a0_p3 = s->a0_p3, af_p3 = s->af_p3; + const double a0_p4 = s->a0_p4, af_p4 = s->af_p4; + const double jMax_jMax = s->jMax_jMax; + + { + double max_a = (-a0 + aMax) / jMax; + double max_b = aMax / jMax; + if (max_a < 0.0) max_a = 0.0; + if (max_b < 0.0) max_b = 0.0; + if (tf < max_a + max_b) { + return false; + } + } + + const double ph1 = 12 * jMax * (-aMax * aMax * vd - jMax * vd_vd + 2 * aMax * jMax * (-pd + tf * vf)); + + /* Profile UDDU */ + { + double polynom[4]; + polynom[0] = (2 * aMax) / jMax; + polynom[1] = (a0_a0 - af_af + 2 * ad * aMax + aMax * aMax + 2 * jMax * (vd - aMax * tf)) / jMax_jMax; + polynom[2] = 0; + polynom[3] = -(-3 * (a0_p4 + af_p4) + 4 * (af_p3 + 2 * a0_p3) * aMax - 12 * a0 * aMax * (af_af - 2 * jMax * vd) + 6 * a0_a0 * (af_af - aMax * aMax - 2 * jMax * vd) + 6 * af_af * (aMax * aMax - 2 * aMax * jMax * tf + 2 * jMax * vd) + ph1) / (12 * jMax_jMax * jMax_jMax); + + const double t_min = -af / jMax; + double t_max_a = tf - (2 * aMax - a0) / jMax; + double t_max_b = -aMin / jMax; + const double t_max = (t_max_a < t_max_b) ? t_max_a : t_max_b; + + CRuckigRootSet roots = cruckig_roots_solve_quart_monic(polynom[0], polynom[1], polynom[2], polynom[3]); + cruckig_root_set_sort(&roots); + for (size_t i = 0; i < roots.size; ++i) { + double t = roots.data[i]; + if (t < t_min || t > t_max) { + continue; + } + + /* Single Newton step (regarding pd) */ + if (t > DBL_EPSILON) { + double h1 = jMax * t * t + vd; + double orig = (-3 * (a0_p4 + af_p4) + 4 * (af_p3 + 2 * a0_p3) * aMax - 24 * af * aMax * jMax_jMax * t * t - 12 * a0 * aMax * (af_af - 2 * jMax * h1) + 6 * a0_a0 * (af_af - aMax * aMax - 2 * jMax * h1) + 6 * af_af * (aMax * aMax - 2 * aMax * jMax * tf + 2 * jMax * h1) - 12 * jMax * (aMax * aMax * h1 + jMax * h1 * h1 + 2 * aMax * jMax * (pd + jMax * t * t * (t - tf) - tf * vf))) / (24 * aMax * jMax_jMax); + double deriv = -t * (a0_a0 - af_af + 2 * aMax * (ad - jMax * tf) + aMax * aMax + 3 * aMax * jMax * t + 2 * jMax * h1) / aMax; + + t -= orig / deriv; + } + + { + const double h1 = ((a0_a0 - af_af) / 2 + jMax * (jMax * t * t + vd)) / aMax; + + profile->t[0] = (-a0 + aMax) / jMax; + profile->t[1] = (h1 - aMax) / jMax; + profile->t[2] = aMax / jMax; + profile->t[3] = tf - (h1 + ad + aMax) / jMax - 2 * t; + profile->t[4] = t; + profile->t[5] = 0; + profile->t[6] = af / jMax + t; + + if (cruckig_profile_check_with_timing(profile, ControlSignsUDDU, ReachedLimitsACC0_VEL, tf, jMax, vMax, vMin, aMax, aMin)) { + return true; + } + } + } + } + + /* Profile UDUD */ + { + double polynom[4]; + polynom[0] = (-2 * aMax) / jMax; + polynom[1] = -(a0_a0 + af_af - 2 * (a0 + af) * aMax + aMax * aMax + 2 * jMax * (vd - aMax * tf)) / jMax_jMax; + polynom[2] = 0; + polynom[3] = (3 * (a0_p4 + af_p4) - 4 * (af_p3 + 2 * a0_p3) * aMax + 6 * a0_a0 * (af_af + aMax * aMax + 2 * jMax * vd) - 12 * a0 * aMax * (af_af + 2 * jMax * vd) + 6 * af_af * (aMax * aMax - 2 * aMax * jMax * tf + 2 * jMax * vd) - ph1) / (12 * jMax_jMax * jMax_jMax); + + const double t_min = af / jMax; + double t_max_a = tf - aMax / jMax; + double t_max_b = aMax / jMax; + const double t_max = (t_max_a < t_max_b) ? t_max_a : t_max_b; + + CRuckigRootSet roots = cruckig_roots_solve_quart_monic(polynom[0], polynom[1], polynom[2], polynom[3]); + cruckig_root_set_sort(&roots); + for (size_t i = 0; i < roots.size; ++i) { + double t = roots.data[i]; + if (t < t_min || t > t_max) { + continue; + } + + /* Single Newton step (regarding pd) */ + { + double h1 = jMax * t * t - vd; + double orig = -(3 * (a0_p4 + af_p4) - 4 * (2 * a0_p3 + af_p3) * aMax + 24 * af * aMax * jMax_jMax * t * t - 12 * a0 * aMax * (af_af - 2 * jMax * h1) + 6 * a0_a0 * (af_af + aMax * aMax - 2 * jMax * h1) + 6 * af_af * (aMax * aMax - 2 * jMax * (tf * aMax + h1)) + 12 * jMax * (-aMax * aMax * h1 + jMax * h1 * h1 - 2 * aMax * jMax * (-pd + jMax * t * t * (t - tf) + tf * vf))) / (24 * aMax * jMax_jMax); + double deriv = t * (a0_a0 + af_af - 2 * jMax * h1 - 2 * (a0 + af + jMax * tf) * aMax + aMax * aMax + 3 * aMax * jMax * t) / aMax; + + t -= orig / deriv; + } + + { + const double h1 = ((a0_a0 + af_af) / 2 + jMax * (vd - jMax * t * t)) / aMax; + + profile->t[0] = (-a0 + aMax) / jMax; + profile->t[1] = (h1 - aMax) / jMax; + profile->t[2] = aMax / jMax; + profile->t[3] = tf - (h1 - a0 - af + aMax) / jMax - 2 * t; + profile->t[4] = t; + profile->t[5] = 0; + profile->t[6] = -(af / jMax) + t; + + if (cruckig_profile_check_with_timing(profile, ControlSignsUDUD, ReachedLimitsACC0_VEL, tf, jMax, vMax, vMin, aMax, aMin)) { + return true; + } + } + } + } + + return false; +} + +static bool time_vel(CRuckigPositionThirdOrderStep2 *s, CRuckigProfile *profile, + double vMax, double vMin, double aMax, double aMin, double jMax) +{ + const double v0 = s->v0, a0 = s->a0, vf = s->vf, af = s->af; + const double tf = s->tf, tf_tf = s->tf_tf; + const double pd = s->pd; + const double vd = s->vd, vd_vd = s->vd_vd; + const double ad = s->ad; + const double a0_a0 = s->a0_a0, af_af = s->af_af; + const double a0_p3 = s->a0_p3, af_p3 = s->af_p3; + const double a0_p4 = s->a0_p4, af_p4 = s->af_p4; + const double a0_p5 = s->a0_p5, a0_p6 = s->a0_p6; + const double af_p6 = s->af_p6; + const double jMax_jMax = s->jMax_jMax; + const double g1 = s->g1; + + double tz_min = -a0 / jMax; + if (tz_min < 0.0) tz_min = 0.0; + double tz_max_a = (tf - a0 / jMax) / 2; + double tz_max_b = (aMax - a0) / jMax; + const double tz_max = (tz_max_a < tz_max_b) ? tz_max_a : tz_max_b; + + /* Profile UDDU */ + if (fabs(v0) < DBL_EPSILON && fabs(a0) < DBL_EPSILON && fabs(vf) < DBL_EPSILON && fabs(af) < DBL_EPSILON) { + double polynom[4]; + polynom[0] = 1; + polynom[1] = -tf / 2; + polynom[2] = 0; + polynom[3] = pd / (2 * jMax); + + CRuckigRootSet roots = cruckig_roots_solve_cubic(polynom[0], polynom[1], polynom[2], polynom[3]); + cruckig_root_set_sort(&roots); + for (size_t i = 0; i < roots.size; ++i) { + double t = roots.data[i]; + if (t > tf / 4) { + continue; + } + + /* Single Newton step (regarding pd) */ + if (t > DBL_EPSILON) { + const double orig = -pd + jMax * t * t * (tf - 2 * t); + const double deriv = 2 * jMax * t * (tf - 3 * t); + t -= orig / deriv; + } + + profile->t[0] = t; + profile->t[1] = 0; + profile->t[2] = t; + profile->t[3] = tf - 4 * t; + profile->t[4] = t; + profile->t[5] = 0; + profile->t[6] = t; + + if (cruckig_profile_check_with_timing(profile, ControlSignsUDDU, ReachedLimitsVEL, tf, jMax, vMax, vMin, aMax, aMin)) { + return true; + } + } + + } else { + const double p1 = af_af - 2 * jMax * (-2 * af * tf + jMax * tf_tf + 3 * vd); + const double ph1 = af_p3 - 3 * jMax_jMax * g1 - 3 * af * jMax * vd; + const double ph2 = af_p4 + 8 * af_p3 * jMax * tf + 12 * jMax * (3 * jMax * vd_vd - af_af * vd + 2 * af * jMax * (g1 - tf * vd) - 2 * jMax_jMax * tf * g1); + const double ph3 = a0 * (af - jMax * tf); + const double ph4 = jMax * (-ad + jMax * tf); + + /* Find root of 5th order polynom */ + double polynom[6]; + polynom[0] = 1.0; + polynom[1] = (15 * a0_a0 + af_af + 4 * af * jMax * tf - 16 * ph3 - 2 * jMax * (jMax * tf_tf + 3 * vd)) / (4 * ph4); + polynom[2] = (29 * a0_p3 - 2 * af_p3 - 33 * a0 * ph3 + 6 * jMax_jMax * g1 + 6 * af * jMax * vd + 6 * a0 * p1) / (6 * jMax * ph4); + polynom[3] = (61 * a0_p4 - 76 * a0_a0 * ph3 - 16 * a0 * ph1 + 30 * a0_a0 * p1 + ph2) / (24 * jMax_jMax * ph4); + polynom[4] = (a0 * (7 * a0_p4 - 10 * a0_a0 * ph3 - 4 * a0 * ph1 + 6 * a0_a0 * p1 + ph2)) / (12 * jMax_jMax * jMax * ph4); + polynom[5] = (7 * a0_p6 + af_p6 - 12 * a0_p4 * ph3 + 48 * af_p3 * jMax_jMax * g1 - 8 * a0_p3 * ph1 - 72 * jMax_jMax * jMax * (jMax * g1 * g1 + vd_vd * vd + 2 * af * g1 * vd) - 6 * af_p4 * jMax * vd + 36 * af_af * jMax_jMax * vd_vd + 9 * a0_p4 * p1 + 3 * a0_a0 * ph2) / (144 * jMax_jMax * jMax_jMax * ph4); + + double deriv[5]; + poly_monic_derivative(polynom, 6, deriv); + double dderiv[4]; + cruckig_roots_poly_derivative(deriv, 5, dderiv); + + /* Solve 4th order derivative analytically */ + CRuckigRootSet d_extremas = cruckig_roots_solve_quart_monic(dderiv[1] / dderiv[0], dderiv[2] / dderiv[0], dderiv[3] / dderiv[0], 0.0); + /* Correct: dderiv has 4 coeffs, the quartic monic needs a,b,c,d where leading is 1 */ + /* Actually dderiv[0] is leading coefficient. Let me recompute correctly */ + /* dderiv = derivative of deriv (which has 5 coefficients, monic). So dderiv has 4 coefficients, not monic. */ + /* For solve_quart_monic, we need the monic form: divide by leading coeff */ + if (fabs(dderiv[0]) > DBL_EPSILON) { + d_extremas = cruckig_roots_solve_quart_monic(dderiv[1] / dderiv[0], dderiv[2] / dderiv[0], dderiv[3] / dderiv[0], 0.0); + } + /* Actually poly_derivative of 5-element monic gives 4 elements, which is a cubic. + We should use solve_cubic instead. */ + /* deriv has 5 elements (degree 4 monic polynomial): deriv[0]=1, ... + dderiv = derivative of deriv -> 4 elements, degree 3 polynomial + dderiv[0] = 4*deriv[0] = 4, dderiv[1] = 3*deriv[1], dderiv[2] = 2*deriv[2], dderiv[3] = 1*deriv[3] + We need to find extremas of deriv, i.e., zeros of dderiv. + d_extremas should come from solve_cubic or solve_quart depending on degree. */ + /* Let's use solve_quart_monic for degree-4 polynomial deriv (which is the correct one for extremas of the 5th order poly). Wait, deriv is degree 4, so its derivative dderiv is degree 3 (4 coefficients). We should use solve_cubic. */ + /* But the C++ code does: d_extremas = roots::solve_quart_monic(deriv[1], deriv[2], deriv[3], deriv[4]); + That solves the quartic x^4 + deriv[1]*x^3 + deriv[2]*x^2 + deriv[3]*x + deriv[4] = 0 + which is finding zeros of deriv (the degree 4 monic polynomial). Those are the extremas of the 5th order polynomial. */ + d_extremas = cruckig_roots_solve_quart_monic(deriv[1], deriv[2], deriv[3], deriv[4]); + + { + double tz_current = tz_min; + cruckig_root_set_sort(&d_extremas); + + for (size_t idx = 0; idx < d_extremas.size; ++idx) { + double tz = d_extremas.data[idx]; + if (tz >= tz_max) { + continue; + } + + { + const double orig_d = cruckig_roots_poly_eval(deriv, 5, tz); + if (fabs(orig_d) > ROOTS_TOLERANCE) { + tz -= orig_d / cruckig_roots_poly_eval(dderiv, 4, tz); + } + } + + const double val_new = cruckig_roots_poly_eval(polynom, 6, tz); + if (fabs(val_new) < 64 * fabs(cruckig_roots_poly_eval(dderiv, 4, tz)) * ROOTS_TOLERANCE) { + /* check_root inline for UDDU */ + double t = tz; + { + const double h1 = sqrt((a0_a0 + af_af) / (2 * jMax_jMax) + (2 * a0 * t + jMax * t * t - vd) / jMax); + const double orig_nr = -pd - (2 * a0_p3 + 4 * af_p3 + 24 * a0 * jMax * t * (af + jMax * (h1 + t - tf)) + 6 * a0_a0 * (af + jMax * (2 * t - tf)) + 6 * (a0_a0 + af_af) * jMax * h1 + 12 * af * jMax * (jMax * t * t - vd) + 12 * jMax_jMax * (jMax * t * t * (h1 + t - tf) - tf * v0 - h1 * vd)) / (12 * jMax_jMax); + const double deriv_newton = -(a0 + jMax * t) * (3 * (h1 + t) - 2 * tf + (a0 + 2 * af) / jMax); + if (!isnan(orig_nr) && !isnan(deriv_newton) && fabs(deriv_newton) > DBL_EPSILON) { + t -= orig_nr / deriv_newton; + } + } + + if (t <= tf && !isnan(t)) { + const double h1 = sqrt((a0_a0 + af_af) / (2 * jMax_jMax) + (t * (2 * a0 + jMax * t) - vd) / jMax); + + profile->t[0] = t; + profile->t[1] = 0; + profile->t[2] = t + a0 / jMax; + profile->t[3] = tf - 2 * (t + h1) - (a0 + af) / jMax; + profile->t[4] = h1; + profile->t[5] = 0; + profile->t[6] = h1 + af / jMax; + + if (cruckig_profile_check_with_timing(profile, ControlSignsUDDU, ReachedLimitsVEL, tf, jMax, vMax, vMin, aMax, aMin)) { + return true; + } + } + } else if (cruckig_roots_poly_eval(polynom, 6, tz_current) * val_new < 0) { + double t = cruckig_roots_shrink_interval(polynom, 6, tz_current, tz); + /* check_root inline for UDDU */ + { + const double h1_sq = (a0_a0 + af_af) / (2 * jMax_jMax) + (2 * a0 * t + jMax * t * t - vd) / jMax; + if (h1_sq >= 0) { + const double h1 = sqrt(h1_sq); + const double orig_nr = -pd - (2 * a0_p3 + 4 * af_p3 + 24 * a0 * jMax * t * (af + jMax * (h1 + t - tf)) + 6 * a0_a0 * (af + jMax * (2 * t - tf)) + 6 * (a0_a0 + af_af) * jMax * h1 + 12 * af * jMax * (jMax * t * t - vd) + 12 * jMax_jMax * (jMax * t * t * (h1 + t - tf) - tf * v0 - h1 * vd)) / (12 * jMax_jMax); + const double deriv_newton = -(a0 + jMax * t) * (3 * (h1 + t) - 2 * tf + (a0 + 2 * af) / jMax); + if (!isnan(orig_nr) && !isnan(deriv_newton) && fabs(deriv_newton) > DBL_EPSILON) { + t -= orig_nr / deriv_newton; + } + } + } + + if (t <= tf && !isnan(t)) { + const double h1 = sqrt((a0_a0 + af_af) / (2 * jMax_jMax) + (t * (2 * a0 + jMax * t) - vd) / jMax); + + profile->t[0] = t; + profile->t[1] = 0; + profile->t[2] = t + a0 / jMax; + profile->t[3] = tf - 2 * (t + h1) - (a0 + af) / jMax; + profile->t[4] = h1; + profile->t[5] = 0; + profile->t[6] = h1 + af / jMax; + + if (cruckig_profile_check_with_timing(profile, ControlSignsUDDU, ReachedLimitsVEL, tf, jMax, vMax, vMin, aMax, aMin)) { + return true; + } + } + } + tz_current = tz; + } + + { + const double val_max = cruckig_roots_poly_eval(polynom, 6, tz_max); + if (cruckig_roots_poly_eval(polynom, 6, tz_current) * val_max < 0) { + double t = cruckig_roots_shrink_interval(polynom, 6, tz_current, tz_max); + { + const double h1_sq = (a0_a0 + af_af) / (2 * jMax_jMax) + (2 * a0 * t + jMax * t * t - vd) / jMax; + if (h1_sq >= 0) { + const double h1 = sqrt(h1_sq); + const double orig_nr = -pd - (2 * a0_p3 + 4 * af_p3 + 24 * a0 * jMax * t * (af + jMax * (h1 + t - tf)) + 6 * a0_a0 * (af + jMax * (2 * t - tf)) + 6 * (a0_a0 + af_af) * jMax * h1 + 12 * af * jMax * (jMax * t * t - vd) + 12 * jMax_jMax * (jMax * t * t * (h1 + t - tf) - tf * v0 - h1 * vd)) / (12 * jMax_jMax); + const double deriv_newton = -(a0 + jMax * t) * (3 * (h1 + t) - 2 * tf + (a0 + 2 * af) / jMax); + if (!isnan(orig_nr) && !isnan(deriv_newton) && fabs(deriv_newton) > DBL_EPSILON) { + t -= orig_nr / deriv_newton; + } + } + } + + if (t <= tf && !isnan(t)) { + const double h1 = sqrt((a0_a0 + af_af) / (2 * jMax_jMax) + (t * (2 * a0 + jMax * t) - vd) / jMax); + + profile->t[0] = t; + profile->t[1] = 0; + profile->t[2] = t + a0 / jMax; + profile->t[3] = tf - 2 * (t + h1) - (a0 + af) / jMax; + profile->t[4] = h1; + profile->t[5] = 0; + profile->t[6] = h1 + af / jMax; + + if (cruckig_profile_check_with_timing(profile, ControlSignsUDDU, ReachedLimitsVEL, tf, jMax, vMax, vMin, aMax, aMin)) { + return true; + } + } + } else if (fabs(val_max) < 8 * DBL_EPSILON) { + double t = tz_max; + { + const double h1_sq = (a0_a0 + af_af) / (2 * jMax_jMax) + (2 * a0 * t + jMax * t * t - vd) / jMax; + if (h1_sq >= 0) { + const double h1 = sqrt(h1_sq); + const double orig_nr = -pd - (2 * a0_p3 + 4 * af_p3 + 24 * a0 * jMax * t * (af + jMax * (h1 + t - tf)) + 6 * a0_a0 * (af + jMax * (2 * t - tf)) + 6 * (a0_a0 + af_af) * jMax * h1 + 12 * af * jMax * (jMax * t * t - vd) + 12 * jMax_jMax * (jMax * t * t * (h1 + t - tf) - tf * v0 - h1 * vd)) / (12 * jMax_jMax); + const double deriv_newton = -(a0 + jMax * t) * (3 * (h1 + t) - 2 * tf + (a0 + 2 * af) / jMax); + if (!isnan(orig_nr) && !isnan(deriv_newton) && fabs(deriv_newton) > DBL_EPSILON) { + t -= orig_nr / deriv_newton; + } + } + } + + if (t <= tf && !isnan(t)) { + const double h1 = sqrt((a0_a0 + af_af) / (2 * jMax_jMax) + (t * (2 * a0 + jMax * t) - vd) / jMax); + + profile->t[0] = t; + profile->t[1] = 0; + profile->t[2] = t + a0 / jMax; + profile->t[3] = tf - 2 * (t + h1) - (a0 + af) / jMax; + profile->t[4] = h1; + profile->t[5] = 0; + profile->t[6] = h1 + af / jMax; + + if (cruckig_profile_check_with_timing(profile, ControlSignsUDDU, ReachedLimitsVEL, tf, jMax, vMax, vMin, aMax, aMin)) { + return true; + } + } + } + } + } + } + + /* Profile UDUD */ + { + const double ph1 = af_af - 2 * jMax * (2 * af * tf + jMax * tf_tf - 3 * vd); + const double ph2 = af_p3 - 3 * jMax_jMax * g1 + 3 * af * jMax * vd; + const double ph3 = 2 * jMax * tf * g1 + 3 * vd_vd; + const double ph4 = af_p4 - 8 * af_p3 * jMax * tf + 12 * jMax * (jMax * ph3 + af_af * vd + 2 * af * jMax * (g1 - tf * vd)); + const double ph5 = af + jMax * tf; + + /* Find root of 6th order polynom */ + double polynom[7]; + polynom[0] = 1.0; + polynom[1] = (5 * a0 - ph5) / jMax; + polynom[2] = (39 * a0_a0 - ph1 - 16 * a0 * ph5) / (4 * jMax_jMax); + polynom[3] = (55 * a0_p3 - 33 * a0_a0 * ph5 - 6 * a0 * ph1 + 2 * ph2) / (6 * jMax_jMax * jMax); + polynom[4] = (101 * a0_p4 + ph4 - 76 * a0_p3 * ph5 - 30 * a0_a0 * ph1 + 16 * a0 * ph2) / (24 * jMax_jMax * jMax_jMax); + polynom[5] = (a0 * (11 * a0_p4 + ph4 - 10 * a0_p3 * ph5 - 6 * a0_a0 * ph1 + 4 * a0 * ph2)) / (12 * jMax_jMax * jMax_jMax * jMax); + polynom[6] = (11 * a0_p6 - af_p6 - 12 * a0_p5 * ph5 - 48 * af_p3 * jMax_jMax * g1 - 9 * a0_p4 * ph1 + 72 * jMax_jMax * jMax * (jMax * g1 * g1 - vd_vd * vd - 2 * af * g1 * vd) - 6 * af_p4 * jMax * vd - 36 * af_af * jMax_jMax * vd_vd + 8 * a0_p3 * ph2 + 3 * a0_a0 * ph4) / (144 * jMax_jMax * jMax_jMax * jMax_jMax); + + double deriv6[6]; /* degree 5 monic poly, derivative of polynom */ + poly_monic_derivative(polynom, 7, deriv6); + + double dderiv6[5]; /* degree 4, derivative of deriv6 */ + poly_monic_derivative(deriv6, 6, dderiv6); + + /* 4th order derivative of dderiv6 (degree 3) */ + double ddderiv6[4]; + cruckig_roots_poly_derivative(dderiv6, 5, ddderiv6); + + /* Solve the 4th order dderiv6 (monic quartic): find zeros of dderiv6 */ + CRuckigRootSet dd_extremas = cruckig_roots_solve_quart_monic(dderiv6[1], dderiv6[2], dderiv6[3], dderiv6[4]); + cruckig_root_set_sort(&dd_extremas); + + /* Build intervals where deriv6 changes sign */ + typedef struct { double first; double second; } interval_pair; + interval_pair dd_tz_intervals[6]; + size_t dd_tz_interval_count = 0; + + double dd_tz_current = tz_min; + for (size_t idx = 0; idx < dd_extremas.size; ++idx) { + double tz = dd_extremas.data[idx]; + if (tz >= tz_max) { + continue; + } + + { + const double orig_dd = cruckig_roots_poly_eval(dderiv6, 5, tz); + if (fabs(orig_dd) > ROOTS_TOLERANCE) { + tz -= orig_dd / cruckig_roots_poly_eval(ddderiv6, 4, tz); + } + } + + if (cruckig_roots_poly_eval(deriv6, 6, dd_tz_current) * cruckig_roots_poly_eval(deriv6, 6, tz) < 0) { + dd_tz_intervals[dd_tz_interval_count].first = dd_tz_current; + dd_tz_intervals[dd_tz_interval_count].second = tz; + dd_tz_interval_count++; + } + dd_tz_current = tz; + } + if (cruckig_roots_poly_eval(deriv6, 6, dd_tz_current) * cruckig_roots_poly_eval(deriv6, 6, tz_max) < 0) { + dd_tz_intervals[dd_tz_interval_count].first = dd_tz_current; + dd_tz_intervals[dd_tz_interval_count].second = tz_max; + dd_tz_interval_count++; + } + + double tz_current = tz_min; + + for (size_t int_idx = 0; int_idx < dd_tz_interval_count; ++int_idx) { + const double tz = cruckig_roots_shrink_interval(deriv6, 6, dd_tz_intervals[int_idx].first, dd_tz_intervals[int_idx].second); + + if (tz >= tz_max) { + continue; + } + + const double p_val = cruckig_roots_poly_eval(polynom, 7, tz); + if (fabs(p_val) < 64 * fabs(cruckig_roots_poly_eval(dderiv6, 5, tz)) * ROOTS_TOLERANCE) { + /* check_root UDUD inline */ + double t = tz; + { + double h1 = sqrt((af_af - a0_a0) / (2 * jMax_jMax) - ((2 * a0 + jMax * t) * t - vd) / jMax); + double orig_nr = -pd + (af_p3 - a0_p3 + 3 * a0_a0 * jMax * (tf - 2 * t)) / (6 * jMax_jMax) + (2 * a0 + jMax * t) * t * (tf - t) + (jMax * h1 - af) * h1 * h1 + tf * v0; + double deriv_newton = (a0 + jMax * t) * (2 * (af + jMax * tf) - 3 * jMax * (h1 + t) - a0) / jMax; + + t -= orig_nr / deriv_newton; + + h1 = sqrt((af_af - a0_a0) / (2 * jMax_jMax) - ((2 * a0 + jMax * t) * t - vd) / jMax); + orig_nr = -pd + (af_p3 - a0_p3 + 3 * a0_a0 * jMax * (tf - 2 * t)) / (6 * jMax_jMax) + (2 * a0 + jMax * t) * t * (tf - t) + (jMax * h1 - af) * h1 * h1 + tf * v0; + if (fabs(orig_nr) > 1e-9) { + deriv_newton = (a0 + jMax * t) * (2 * (af + jMax * tf) - 3 * jMax * (h1 + t) - a0) / jMax; + t -= orig_nr / deriv_newton; + } + } + + { + const double h1 = sqrt((af_af - a0_a0) / (2 * jMax_jMax) - ((2 * a0 + jMax * t) * t - vd) / jMax); + + profile->t[0] = t; + profile->t[1] = 0; + profile->t[2] = t + a0 / jMax; + profile->t[3] = tf - 2 * (t + h1) + ad / jMax; + profile->t[4] = h1; + profile->t[5] = 0; + profile->t[6] = h1 - af / jMax; + + if (cruckig_profile_check_with_timing(profile, ControlSignsUDUD, ReachedLimitsVEL, tf, jMax, vMax, vMin, aMax, aMin)) { + return true; + } + } + } else if (cruckig_roots_poly_eval(polynom, 7, tz_current) * p_val < 0) { + double t = cruckig_roots_shrink_interval(polynom, 7, tz_current, tz); + /* check_root UDUD inline */ + { + double h1 = sqrt((af_af - a0_a0) / (2 * jMax_jMax) - ((2 * a0 + jMax * t) * t - vd) / jMax); + double orig_nr = -pd + (af_p3 - a0_p3 + 3 * a0_a0 * jMax * (tf - 2 * t)) / (6 * jMax_jMax) + (2 * a0 + jMax * t) * t * (tf - t) + (jMax * h1 - af) * h1 * h1 + tf * v0; + double deriv_newton = (a0 + jMax * t) * (2 * (af + jMax * tf) - 3 * jMax * (h1 + t) - a0) / jMax; + + t -= orig_nr / deriv_newton; + + h1 = sqrt((af_af - a0_a0) / (2 * jMax_jMax) - ((2 * a0 + jMax * t) * t - vd) / jMax); + orig_nr = -pd + (af_p3 - a0_p3 + 3 * a0_a0 * jMax * (tf - 2 * t)) / (6 * jMax_jMax) + (2 * a0 + jMax * t) * t * (tf - t) + (jMax * h1 - af) * h1 * h1 + tf * v0; + if (fabs(orig_nr) > 1e-9) { + deriv_newton = (a0 + jMax * t) * (2 * (af + jMax * tf) - 3 * jMax * (h1 + t) - a0) / jMax; + t -= orig_nr / deriv_newton; + } + } + + { + const double h1 = sqrt((af_af - a0_a0) / (2 * jMax_jMax) - ((2 * a0 + jMax * t) * t - vd) / jMax); + + profile->t[0] = t; + profile->t[1] = 0; + profile->t[2] = t + a0 / jMax; + profile->t[3] = tf - 2 * (t + h1) + ad / jMax; + profile->t[4] = h1; + profile->t[5] = 0; + profile->t[6] = h1 - af / jMax; + + if (cruckig_profile_check_with_timing(profile, ControlSignsUDUD, ReachedLimitsVEL, tf, jMax, vMax, vMin, aMax, aMin)) { + return true; + } + } + } + tz_current = tz; + } + + if (cruckig_roots_poly_eval(polynom, 7, tz_current) * cruckig_roots_poly_eval(polynom, 7, tz_max) < 0) { + double t = cruckig_roots_shrink_interval(polynom, 7, tz_current, tz_max); + { + double h1 = sqrt((af_af - a0_a0) / (2 * jMax_jMax) - ((2 * a0 + jMax * t) * t - vd) / jMax); + double orig_nr = -pd + (af_p3 - a0_p3 + 3 * a0_a0 * jMax * (tf - 2 * t)) / (6 * jMax_jMax) + (2 * a0 + jMax * t) * t * (tf - t) + (jMax * h1 - af) * h1 * h1 + tf * v0; + double deriv_newton = (a0 + jMax * t) * (2 * (af + jMax * tf) - 3 * jMax * (h1 + t) - a0) / jMax; + + t -= orig_nr / deriv_newton; + + h1 = sqrt((af_af - a0_a0) / (2 * jMax_jMax) - ((2 * a0 + jMax * t) * t - vd) / jMax); + orig_nr = -pd + (af_p3 - a0_p3 + 3 * a0_a0 * jMax * (tf - 2 * t)) / (6 * jMax_jMax) + (2 * a0 + jMax * t) * t * (tf - t) + (jMax * h1 - af) * h1 * h1 + tf * v0; + if (fabs(orig_nr) > 1e-9) { + deriv_newton = (a0 + jMax * t) * (2 * (af + jMax * tf) - 3 * jMax * (h1 + t) - a0) / jMax; + t -= orig_nr / deriv_newton; + } + } + + { + const double h1 = sqrt((af_af - a0_a0) / (2 * jMax_jMax) - ((2 * a0 + jMax * t) * t - vd) / jMax); + + profile->t[0] = t; + profile->t[1] = 0; + profile->t[2] = t + a0 / jMax; + profile->t[3] = tf - 2 * (t + h1) + ad / jMax; + profile->t[4] = h1; + profile->t[5] = 0; + profile->t[6] = h1 - af / jMax; + + if (cruckig_profile_check_with_timing(profile, ControlSignsUDUD, ReachedLimitsVEL, tf, jMax, vMax, vMin, aMax, aMin)) { + return true; + } + } + } + } + + return false; +} + +static bool time_acc0_acc1(CRuckigPositionThirdOrderStep2 *s, CRuckigProfile *profile, + double vMax, double vMin, double aMax, double aMin, double jMax) +{ + const double v0 = s->v0, a0 = s->a0, vf = s->vf, af = s->af; + const double tf = s->tf, tf_tf = s->tf_tf; + const double pd = s->pd; + const double vd = s->vd, vd_vd = s->vd_vd; + const double ad = s->ad; + const double a0_a0 = s->a0_a0, af_af = s->af_af; + const double a0_p3 = s->a0_p3, af_p3 = s->af_p3; + const double g1 = s->g1, g2 = s->g2; + + if (fabs(a0) < DBL_EPSILON && fabs(af) < DBL_EPSILON) { + const double h1 = 2 * aMin * g1 + vd_vd + aMax * (2 * pd + aMin * tf_tf - 2 * tf * vf); + const double h2 = ((aMax - aMin) * (-aMin * vd + aMax * (aMin * tf - vd))); + + const double jf = h2 / h1; + profile->t[0] = aMax / jf; + profile->t[1] = (-2 * aMax * h1 + aMin * aMin * g2) / h2; + profile->t[2] = profile->t[0]; + profile->t[3] = 0; + profile->t[4] = -aMin / jf; + profile->t[5] = tf - (2 * profile->t[0] + profile->t[1] + 2 * profile->t[4]); + profile->t[6] = profile->t[4]; + + return cruckig_profile_check_with_timing_full(profile, ControlSignsUDDU, ReachedLimitsACC0_ACC1, tf, jf, vMax, vMin, aMax, aMin, jMax); + } + + /* UDDU */ + { + const double h1 = sqrt(144 * cruckig_pow2((aMax - aMin) * (-aMin * vd + aMax * (aMin * tf - vd)) - af_af * (aMax * tf - vd) + 2 * af * aMin * (aMax * tf - vd) + a0_a0 * (aMin * tf + v0 - vf) - 2 * a0 * aMax * (aMin * tf - vd)) + 48 * ad * (3 * a0_p3 - 3 * af_p3 + 12 * aMax * aMin * (-aMax + aMin) + 4 * af_af * (aMax + 2 * aMin) + a0 * (-3 * af_af + 8 * af * (aMin - aMax) + 6 * (aMax * aMax + 2 * aMax * aMin - aMin * aMin)) + 6 * af * (aMax * aMax - 2 * aMax * aMin - aMin * aMin) + a0_a0 * (3 * af - 4 * (2 * aMax + aMin))) * (2 * aMin * g1 + vd * vd + aMax * (2 * pd + aMin * tf * tf - 2 * tf * vf))); + + const double jf = -(3 * af_af * aMax * tf - 3 * a0_a0 * aMin * tf - 6 * ad * aMax * aMin * tf + 3 * aMax * aMin * (aMin - aMax) * tf + 3 * (a0_a0 - af_af) * vd + 6 * vd * (af * aMin - a0 * aMax) + 3 * (aMax * aMax - aMin * aMin) * vd + h1 / 4) / (6 * (2 * aMin * g1 + vd * vd + aMax * (2 * pd + aMin * tf_tf - 2 * tf * vf))); + profile->t[0] = (aMax - a0) / jf; + profile->t[1] = (a0_a0 - af_af + 2 * ad * aMin - 2 * (aMax * aMax - 2 * aMax * aMin + aMin * aMin + aMin * jf * tf - jf * vd)) / (2 * (aMax - aMin) * jf); + profile->t[2] = aMax / jf; + profile->t[3] = 0; + profile->t[4] = -aMin / jf; + profile->t[5] = tf - (profile->t[0] + profile->t[1] + profile->t[2] + 2 * profile->t[4] + af / jf); + profile->t[6] = profile->t[4] + af / jf; + + if (cruckig_profile_check_with_timing_full(profile, ControlSignsUDDU, ReachedLimitsACC0_ACC1, tf, jf, vMax, vMin, aMax, aMin, jMax)) { + return true; + } + } + + return false; +} + +static bool time_acc1(CRuckigPositionThirdOrderStep2 *s, CRuckigProfile *profile, + double vMax, double vMin, double aMax, double aMin, double jMax) +{ + const double a0 = s->a0, vf = s->vf, af = s->af; + const double tf = s->tf, tf_tf = s->tf_tf; + const double pd = s->pd; + const double vd = s->vd, vd_vd = s->vd_vd; + const double ad = s->ad; + const double a0_a0 = s->a0_a0, af_af = s->af_af; + const double a0_p3 = s->a0_p3, af_p3 = s->af_p3; + const double a0_p4 = s->a0_p4, af_p4 = s->af_p4; + const double jMax_jMax = s->jMax_jMax; + const double g1 = s->g1, g2 = s->g2; + + /* Case UDDU */ + { + const double h0 = sqrt(jMax_jMax * (a0_p4 + af_p4 - 4 * af_p3 * jMax * tf + 6 * af_af * jMax_jMax * tf_tf - 4 * a0_p3 * (af - jMax * tf) + 6 * a0_a0 * (af - jMax * tf) * (af - jMax * tf) + 24 * af * jMax_jMax * g1 - 4 * a0 * (af_p3 - 3 * af_af * jMax * tf + 6 * jMax_jMax * (-pd + tf * vf)) - 12 * jMax_jMax * (-vd_vd + jMax * tf * g2)) / 3) / jMax; + const double h1 = sqrt((a0_a0 + af_af - 2 * a0 * af - 2 * ad * jMax * tf + 2 * h0) / jMax_jMax + tf_tf); + + profile->t[0] = -(a0_a0 + af_af + 2 * a0 * (jMax * tf - af) - 2 * jMax * vd + h0) / (2 * jMax * (-ad + jMax * tf)); + profile->t[1] = 0; + profile->t[2] = (tf - h1) / 2 - ad / (2 * jMax); + profile->t[3] = 0; + profile->t[4] = 0; + profile->t[5] = h1; + profile->t[6] = tf - (profile->t[0] + profile->t[2] + profile->t[5]); + + if (cruckig_profile_check_with_timing(profile, ControlSignsUDDU, ReachedLimitsACC1, tf, jMax, vMax, vMin, aMax, aMin)) { + return true; + } + } + + /* Case UDUD */ + { + const double h0 = sqrt(jMax_jMax * (a0_p4 + af_p4 + 4 * (af_p3 - a0_p3) * jMax * tf + 6 * af_af * jMax_jMax * tf_tf + 6 * a0_a0 * (af + jMax * tf) * (af + jMax * tf) + 24 * af * jMax_jMax * g1 - 4 * a0 * (a0_a0 * af + af_p3 + 3 * af_af * jMax * tf + 6 * jMax_jMax * (-pd + tf * vf)) + 12 * jMax_jMax * (vd_vd + jMax * tf * g2)) / 3) / jMax; + const double h1 = sqrt((a0_a0 + af_af - 2 * a0 * af + 2 * ad * jMax * tf + 2 * h0) / jMax_jMax + tf_tf); + + profile->t[0] = 0; + profile->t[1] = 0; + profile->t[2] = -(a0_a0 + af_af - 2 * a0 * af + 2 * jMax * (vd - a0 * tf) + h0) / (2 * jMax * (ad + jMax * tf)); + profile->t[3] = 0; + profile->t[4] = ad / (2 * jMax) + (tf - h1) / 2; + profile->t[5] = h1; + profile->t[6] = tf - (profile->t[5] + profile->t[4] + profile->t[2]); + + if (cruckig_profile_check_with_timing(profile, ControlSignsUDUD, ReachedLimitsACC1, tf, jMax, vMax, vMin, aMax, aMin)) { + return true; + } + } + + /* Case UDDU, Solution 2 */ + { + const double h0a = a0_p3 - af_p3 - 3 * a0_a0 * aMin + 3 * aMin * aMin * (a0 + jMax * tf) + 3 * af * aMin * (-aMin - 2 * jMax * tf) - 3 * af_af * (-aMin - jMax * tf) - 3 * jMax_jMax * (-2 * pd - aMin * tf_tf + 2 * tf * vf); + const double h0b = a0_a0 + af_af - 2 * (a0 + af) * aMin + 2 * (aMin * aMin - jMax * (-aMin * tf + vd)); + const double h0c = a0_p4 + 3 * af_p4 - 4 * (a0_p3 + 2 * af_p3) * aMin + 6 * a0_a0 * aMin * aMin + 6 * af_af * (aMin * aMin - 2 * jMax * vd) + 12 * jMax * (2 * aMin * jMax * g1 - aMin * aMin * vd + jMax * vd_vd) + 24 * af * aMin * jMax * vd - 4 * a0 * (af_p3 - 3 * af * aMin * (-aMin - 2 * jMax * tf) + 3 * af_af * (-aMin - jMax * tf) + 3 * jMax * (-aMin * aMin * tf + jMax * (-2 * pd - aMin * tf_tf + 2 * tf * vf))); + const double h1 = fabs(jMax) / jMax * sqrt(4 * h0a * h0a - 6 * h0b * h0c); + const double h2 = 6 * jMax * h0b; + + profile->t[0] = 0; + profile->t[1] = 0; + profile->t[2] = (2 * h0a + h1) / h2; + profile->t[3] = -(a0_a0 + af_af - 2 * (a0 + af) * aMin + 2 * (aMin * aMin + aMin * jMax * tf - jMax * vd)) / (2 * jMax * (a0 - aMin - jMax * profile->t[2])); + profile->t[4] = (a0 - aMin) / jMax - profile->t[2]; + profile->t[5] = tf - (profile->t[2] + profile->t[3] + profile->t[4] + (af - aMin) / jMax); + profile->t[6] = (af - aMin) / jMax; + + if (cruckig_profile_check_with_timing(profile, ControlSignsUDDU, ReachedLimitsACC1, tf, jMax, vMax, vMin, aMax, aMin)) { + return true; + } + } + + /* Case UDUD, Solution 1 */ + { + const double h0a = -a0_p3 + af_p3 + 3 * (a0_a0 - af_af) * aMax - 3 * ad * aMax * aMax - 6 * af * aMax * jMax * tf + 3 * af_af * jMax * tf + 3 * jMax * (aMax * aMax * tf + jMax * (-2 * pd - aMax * tf_tf + 2 * tf * vf)); + const double h0b = a0_a0 - af_af + 2 * ad * aMax + 2 * jMax * (aMax * tf - vd); + const double h0c = a0_p4 + 3 * af_p4 - 4 * (a0_p3 + 2 * af_p3) * aMax + 6 * a0_a0 * aMax * aMax - 24 * af * aMax * jMax * vd + 12 * jMax * (2 * aMax * jMax * g1 + jMax * vd_vd + aMax * aMax * vd) + 6 * af_af * (aMax * aMax + 2 * jMax * vd) - 4 * a0 * (af_p3 + 3 * af * aMax * (aMax - 2 * jMax * tf) - 3 * af_af * (aMax - jMax * tf) + 3 * jMax * (aMax * aMax * tf + jMax * (-2 * pd - aMax * tf_tf + 2 * tf * vf))); + const double h1 = fabs(jMax) / jMax * sqrt(4 * h0a * h0a - 6 * h0b * h0c); + const double h2 = 6 * jMax * h0b; + + profile->t[0] = 0; + profile->t[1] = 0; + profile->t[2] = -(2 * h0a + h1) / h2; + profile->t[3] = 2 * h1 / h2; + profile->t[4] = (aMax - a0) / jMax + profile->t[2]; + profile->t[5] = tf - (profile->t[2] + profile->t[3] + profile->t[4] + (-af + aMax) / jMax); + profile->t[6] = (-af + aMax) / jMax; + + if (cruckig_profile_check_with_timing(profile, ControlSignsUDUD, ReachedLimitsACC1, tf, jMax, vMax, vMin, aMax, aMin)) { + return true; + } + } + return false; +} + +static bool time_acc0(CRuckigPositionThirdOrderStep2 *s, CRuckigProfile *profile, + double vMax, double vMin, double aMax, double aMin, double jMax) +{ + const double v0 = s->v0, a0 = s->a0, af = s->af; + const double tf = s->tf, tf_tf = s->tf_tf; + const double pd = s->pd; + const double vd = s->vd; + const double ad = s->ad, ad_ad = s->ad_ad; + const double a0_a0 = s->a0_a0, af_af = s->af_af; + const double a0_p3 = s->a0_p3, af_p3 = s->af_p3; + const double jMax_jMax = s->jMax_jMax; + const double g1 = s->g1, g2 = s->g2; + + /* UDUD */ + { + const double h1 = sqrt(ad_ad / (2 * jMax_jMax) - ad * (aMax - a0) / (jMax_jMax) + (aMax * tf - vd) / jMax); + + profile->t[0] = (aMax - a0) / jMax; + profile->t[1] = tf - ad / jMax - 2 * h1; + profile->t[2] = h1; + profile->t[3] = 0; + profile->t[4] = (af - aMax) / jMax + h1; + profile->t[5] = 0; + profile->t[6] = 0; + + if (cruckig_profile_check_with_timing(profile, ControlSignsUDUD, ReachedLimitsNONE, tf, jMax, vMax, vMin, aMax, aMin)) { + return true; + } + } + + /* UDUD - second */ + { + const double h0a = -a0_a0 + af_af - 2 * ad * aMax + 2 * jMax * (aMax * tf - vd); + const double h0b = a0_p3 + 2 * af_p3 - 6 * af_af * aMax - 3 * a0_a0 * (af - jMax * tf) - 3 * a0 * aMax * (aMax - 2 * af + 2 * jMax * tf) - 3 * jMax * (jMax * (-2 * pd + aMax * tf_tf + 2 * tf * v0) + aMax * (aMax * tf - 2 * vd)) + 3 * af * (aMax * aMax + 2 * aMax * jMax * tf - 2 * jMax * vd); + const double h0 = fabs(jMax) * sqrt(4 * h0b * h0b - 18 * h0a * h0a * h0a); + const double h1 = 3 * jMax * h0a; + + profile->t[0] = (-a0 + aMax) / jMax; + profile->t[1] = (-a0_p3 + af_p3 + af_af * (-6 * aMax + 3 * jMax * tf) + a0_a0 * (-3 * af + 6 * aMax + 3 * jMax * tf) + 6 * af * (aMax * aMax - jMax * vd) + 3 * a0 * (af_af - 2 * (aMax * aMax + jMax * vd)) - 6 * jMax * (aMax * (aMax * tf - 2 * vd) + jMax * g2)) / h1; + profile->t[2] = -(ad + h0 / h1) / (2 * jMax) + tf / 2 - profile->t[1] / 2; + profile->t[3] = h0 / (jMax * h1); + profile->t[4] = 0; + profile->t[5] = 0; + profile->t[6] = tf - (profile->t[0] + profile->t[1] + profile->t[2] + profile->t[3]); + + if (cruckig_profile_check_with_timing(profile, ControlSignsUDDU, ReachedLimitsNONE, tf, jMax, vMax, vMin, aMax, aMin)) { + return true; + } + } + + /* a3 != 0 */ + /* UDDU Solution 1 */ + { + const double h0a = a0_p3 + 2 * af_p3 - 6 * (af_af + aMax * aMax) * aMax - 6 * (a0 + af) * aMax * jMax * tf + 9 * aMax * aMax * (af + jMax * tf) + 3 * a0 * aMax * (-2 * af + 3 * aMax) + 3 * a0_a0 * (af - 2 * aMax + jMax * tf) - 6 * jMax_jMax * g1 + 6 * (af - aMax) * jMax * vd - 3 * aMax * jMax_jMax * tf_tf; + const double h0b = a0_a0 + af_af + 2 * (aMax * aMax - (a0 + af) * aMax + jMax * (vd - aMax * tf)); + const double h1 = fabs(jMax) / jMax * sqrt(4 * h0a * h0a - 18 * h0b * h0b * h0b); + const double h2 = 6 * jMax * h0b; + + profile->t[0] = (-a0 + aMax) / jMax; + profile->t[1] = ad / jMax - 2 * profile->t[0] - (2 * h0a - h1) / h2 + tf; + profile->t[2] = -(2 * h0a + h1) / h2; + profile->t[3] = (2 * h0a - h1) / h2; + profile->t[4] = tf - (profile->t[0] + profile->t[1] + profile->t[2] + profile->t[3]); + profile->t[5] = 0; + profile->t[6] = 0; + + if (cruckig_profile_check_with_timing(profile, ControlSignsUDDU, ReachedLimitsACC0, tf, jMax, vMax, vMin, aMax, aMin)) { + return true; + } + } + + return false; +} + +static bool time_none(CRuckigPositionThirdOrderStep2 *s, CRuckigProfile *profile, + double vMax, double vMin, double aMax, double aMin, double jMax) +{ + const double v0 = s->v0, a0 = s->a0, vf = s->vf, af = s->af; + const double tf = s->tf, tf_tf = s->tf_tf, tf_p3 = s->tf_p3, tf_p4 = s->tf_p4; + const double pd = s->pd; + const double vd = s->vd, vd_vd = s->vd_vd; + const double vf_vf = s->vf_vf; + const double ad = s->ad, ad_ad = s->ad_ad; + const double a0_a0 = s->a0_a0, af_af = s->af_af; + const double a0_p3 = s->a0_p3, af_p3 = s->af_p3; + const double a0_p4 = s->a0_p4, af_p4 = s->af_p4; + const double a0_p5 = s->a0_p5, a0_p6 = s->a0_p6; + const double af_p5 = s->af_p5, af_p6 = s->af_p6; + const double jMax_jMax = s->jMax_jMax; + const double g1 = s->g1, g2 = s->g2; + + if (fabs(v0) < DBL_EPSILON && fabs(a0) < DBL_EPSILON && fabs(af) < DBL_EPSILON) { + const double h1 = sqrt(tf_tf * vf_vf + cruckig_pow2(4 * pd - tf * vf)); + const double jf = 4 * (4 * pd - 2 * tf * vf + h1) / tf_p3; + + profile->t[0] = tf / 4; + profile->t[1] = 0; + profile->t[2] = 2 * profile->t[0]; + profile->t[3] = 0; + profile->t[4] = 0; + profile->t[5] = 0; + profile->t[6] = profile->t[0]; + + if (cruckig_profile_check_with_timing_full(profile, ControlSignsUDDU, ReachedLimitsNONE, tf, jf, vMax, vMin, aMax, aMin, jMax)) { + return true; + } + } + + if (fabs(a0) < DBL_EPSILON && fabs(af) < DBL_EPSILON) { + /* Profiles with a3 != 0, Solution UDDU */ + { + /* First acc, then constant */ + { + double polynom[4]; + polynom[0] = -2 * tf; + polynom[1] = 2 * vd / jMax + tf_tf; + polynom[2] = 4 * (pd - tf * vf) / jMax; + polynom[3] = (vd_vd + jMax * tf * g2) / (jMax_jMax); + + CRuckigRootSet roots = cruckig_roots_solve_quart_monic(polynom[0], polynom[1], polynom[2], polynom[3]); + cruckig_root_set_sort(&roots); + for (size_t i = 0; i < roots.size; ++i) { + double t = roots.data[i]; + if (t > tf / 2 || t > (aMax - a0) / jMax) { + continue; + } + + /* Single Newton step (regarding pd) */ + { + const double h1 = (jMax * t * (t - tf) + vd) / (jMax * (2 * t - tf)); + const double h2 = (2 * jMax * t * (t - tf) + jMax * tf_tf - 2 * vd) / (jMax * (2 * t - tf) * (2 * t - tf)); + const double orig = (-2 * pd + 2 * tf * v0 + h1 * h1 * jMax * (tf - 2 * t) + jMax * tf * (2 * h1 * t - t * t - (h1 - t) * tf)) / 2; + const double deriv_n = (jMax * tf * (2 * t - tf) * (h2 - 1)) / 2 + h1 * jMax * (tf - (2 * t - tf) * h2 - h1); + + t -= orig / deriv_n; + } + + profile->t[0] = t; + profile->t[1] = 0; + profile->t[2] = (jMax * t * (t - tf) + vd) / (jMax * (2 * t - tf)); + profile->t[3] = tf - 2 * t; + profile->t[4] = t - profile->t[2]; + profile->t[5] = 0; + profile->t[6] = 0; + + if (cruckig_profile_check_with_timing(profile, ControlSignsUDDU, ReachedLimitsNONE, tf, jMax, vMax, vMin, aMax, aMin)) { + return true; + } + } + } + } + } + + /* UDUD T 0246 */ + { + const double h0 = sqrt(2 * jMax_jMax * (2 * cruckig_pow2(a0_p3 - af_p3 - 3 * af_af * jMax * tf + 9 * af * jMax_jMax * tf_tf - 3 * a0_a0 * (af + jMax * tf) + 3 * a0 * cruckig_pow2(af + jMax * tf) + 3 * jMax_jMax * (8 * pd + jMax * tf_tf * tf - 8 * tf * vf)) - 3 * (a0_a0 + af_af - 2 * af * jMax * tf - 2 * a0 * (af + jMax * tf) - jMax * (jMax * tf_tf + 4 * v0 - 4 * vf)) * (a0_p4 + af_p4 + 4 * af_p3 * jMax * tf + 6 * af_af * jMax_jMax * tf_tf - 3 * jMax_jMax * jMax_jMax * tf_tf * tf_tf - 4 * a0_p3 * (af + jMax * tf) + 6 * a0_a0 * cruckig_pow2(af + jMax * tf) - 12 * af * jMax_jMax * (8 * pd + jMax * tf_tf * tf - 8 * tf * v0) + 48 * jMax_jMax * vd_vd + 48 * jMax_jMax * jMax * tf * g2 - 4 * a0 * (af_p3 + 3 * af_af * jMax * tf - 9 * af * jMax_jMax * tf_tf - 3 * jMax_jMax * (8 * pd + jMax * tf_tf * tf - 8 * tf * vf))))) / jMax; + const double h1 = 12 * jMax * (-a0_a0 - af_af + 2 * af * jMax * tf + 2 * a0 * (af + jMax * tf) + jMax * (jMax * tf_tf + 4 * v0 - 4 * vf)); + const double h2 = -4 * a0_p3 + 4 * af_p3 + 12 * a0_a0 * af - 12 * a0 * af_af + 48 * jMax_jMax * pd + 12 * (a0_a0 - af_af) * jMax * tf - 24 * jMax_jMax * tf * (v0 + vf) + 24 * ad * jMax * vd; + const double h3 = 2 * a0_p3 - 2 * af_p3 - 6 * a0_a0 * af + 6 * a0 * af_af; + + profile->t[0] = (h3 - 48 * jMax_jMax * (tf * vf - pd) - 6 * (a0_a0 + af_af) * jMax * tf + 12 * a0 * af * jMax * tf + 6 * (a0 + 3 * af + jMax * tf) * tf_tf * jMax_jMax - h0) / h1; + profile->t[1] = 0; + profile->t[2] = (h2 + h0) / h1; + profile->t[3] = 0; + profile->t[4] = (-h2 + h0) / h1; + profile->t[5] = 0; + profile->t[6] = (-h3 + 48 * jMax_jMax * (tf * v0 - pd) - 6 * (a0_a0 + af_af) * jMax * tf + 12 * a0 * af * jMax * tf + 6 * (af + 3 * a0 + jMax * tf) * tf_tf * jMax_jMax - h0) / h1; + + if (cruckig_profile_check_with_timing(profile, ControlSignsUDUD, ReachedLimitsNONE, tf, jMax, vMax, vMin, aMax, aMin)) { + return true; + } + } + + /* Profiles with a3 != 0, Solution UDDU */ + { + /* T 0234 */ + { + const double ph1 = af + jMax * tf; + + double polynom[4]; + polynom[0] = -2 * (ad + jMax * tf) / jMax; + polynom[1] = 2 * (a0_a0 + af_af + jMax * (af * tf + vd) - 2 * a0 * ph1) / jMax_jMax + tf_tf; + polynom[2] = 2 * (a0_p3 - af_p3 - 3 * af_af * jMax * tf + 3 * a0 * ph1 * (ph1 - a0) - 6 * jMax_jMax * (-pd + tf * vf)) / (3 * jMax_jMax * jMax); + polynom[3] = (a0_p4 + af_p4 + 4 * af_p3 * jMax * tf - 4 * a0_p3 * ph1 + 6 * a0_a0 * ph1 * ph1 + 24 * jMax_jMax * af * g1 - 4 * a0 * (af_p3 + 3 * af_af * jMax * tf + 6 * jMax_jMax * (-pd + tf * vf)) + 6 * jMax_jMax * af_af * tf_tf + 12 * jMax_jMax * (vd_vd + jMax * tf * g2)) / (12 * jMax_jMax * jMax_jMax); + + const double t_min = ad / jMax; + double t_max_a = (aMax - a0) / jMax; + double t_max_b = (ad / jMax + tf) / 2; + const double t_max = (t_max_a < t_max_b) ? t_max_a : t_max_b; + + CRuckigRootSet roots = cruckig_roots_solve_quart_monic(polynom[0], polynom[1], polynom[2], polynom[3]); + cruckig_root_set_sort(&roots); + for (size_t i = 0; i < roots.size; ++i) { + double t = roots.data[i]; + if (t < t_min || t > t_max) { + continue; + } + + /* Single Newton step (regarding pd) */ + { + const double h0 = jMax * (2 * t - tf) - ad; + const double h1_v = (ad_ad - 2 * af * jMax * t + 2 * a0 * jMax * (t - tf) + 2 * jMax * (jMax * t * (t - tf) + vd)) / (2 * jMax * h0); + const double h2_v = (-ad_ad + 2 * jMax_jMax * (tf_tf + t * (t - tf)) + (a0 + af) * jMax * tf - ad * h0 - 2 * jMax * vd) / (h0 * h0); + const double orig = (-a0_p3 + af_p3 + 3 * ad_ad * jMax * (h1_v - t) + 3 * ad * jMax_jMax * (h1_v - t) * (h1_v - t) - 3 * a0 * af * ad + 3 * jMax_jMax * (a0 * tf_tf - 2 * pd + 2 * tf * v0 + h1_v * h1_v * jMax * (tf - 2 * t) + jMax * tf * (2 * h1_v * t - t * t - (h1_v - t) * tf))) / (6 * jMax_jMax); + const double deriv_n = (h0 * (-ad + jMax * tf) * (h2_v - 1)) / (2 * jMax) + h1_v * (-ad + jMax * (tf - h1_v) - h0 * h2_v); + + t -= orig / deriv_n; + } + + profile->t[0] = t; + profile->t[1] = 0; + profile->t[2] = (ad_ad + 2 * jMax * (-a0 * tf - ad * t + jMax * t * (t - tf) + vd)) / (2 * jMax * (-ad + jMax * (2 * t - tf))); + profile->t[3] = ad / jMax + tf - 2 * t; + profile->t[4] = tf - (t + profile->t[2] + profile->t[3]); + profile->t[5] = 0; + profile->t[6] = 0; + + if (cruckig_profile_check_with_timing(profile, ControlSignsUDDU, ReachedLimitsNONE, tf, jMax, vMax, vMin, aMax, aMin)) { + return true; + } + } + } + + /* T 3456 */ + { + const double h1_val = 3 * jMax * (ad_ad + 2 * jMax * (a0 * tf - vd)); + const double h2_val = ad_ad + 2 * jMax * (a0 * tf - vd); + const double h0 = sqrt(4 * cruckig_pow2(2 * (a0_p3 - af_p3) - 6 * a0_a0 * (af - jMax * tf) + 6 * jMax_jMax * g1 + 3 * a0 * (2 * af_af - 2 * jMax * af * tf + jMax_jMax * tf_tf) + 6 * ad * jMax * vd) - 18 * h2_val * h2_val * h2_val) / h1_val * fabs(jMax) / jMax; + + profile->t[0] = 0; + profile->t[1] = 0; + profile->t[2] = 0; + profile->t[3] = (af_p3 - a0_p3 + 3 * (af_af - a0_a0) * jMax * tf - 3 * ad * (a0 * af + 2 * jMax * vd) - 6 * jMax_jMax * g2) / h1_val; + profile->t[4] = (tf - profile->t[3] - h0) / 2 - ad / (2 * jMax); + profile->t[5] = h0; + profile->t[6] = (tf - profile->t[3] + ad / jMax - h0) / 2; + + if (cruckig_profile_check_with_timing(profile, ControlSignsUDDU, ReachedLimitsNONE, tf, jMax, vMax, vMin, aMax, aMin)) { + return true; + } + } + + /* T 2346 */ + { + const double ph1 = ad_ad + 2 * (af + a0) * jMax * tf - jMax * (jMax * tf_tf + 4 * vd); + const double ph2 = jMax * tf_tf * g1 - vd * (-2 * pd - tf * v0 + 3 * tf * vf); + const double ph3 = 5 * af_af - 8 * af * jMax * tf + 2 * jMax * (2 * jMax * tf_tf - vd); + const double ph4 = jMax_jMax * tf_p4 - 2 * vd_vd + 8 * jMax * tf * (-pd + tf * vf); + const double ph5 = (5 * af_p4 - 8 * af_p3 * jMax * tf - 12 * af_af * jMax * (jMax * tf_tf + vd) + 24 * af * jMax_jMax * (-2 * pd + jMax * tf_p3 + 2 * tf * vf) - 6 * jMax_jMax * ph4); + const double ph6 = -vd_vd + jMax * tf * (-2 * pd + 3 * tf * v0 - tf * vf) - af * g2; + + double polynom[4]; + polynom[0] = -(4 * (a0_p3 - af_p3) - 12 * a0_a0 * (af - jMax * tf) + 6 * a0 * (2 * af_af - 2 * af * jMax * tf + jMax * (jMax * tf_tf - 2 * vd)) + 6 * af * jMax * (3 * jMax * tf_tf + 2 * vd) - 6 * jMax_jMax * (-4 * pd + jMax * tf_p3 - 2 * tf * v0 + 6 * tf * vf)) / (3 * jMax * ph1); + polynom[1] = -(-a0_p4 - af_p4 + 4 * a0_p3 * (af - jMax * tf) + a0_a0 * (-6 * af_af + 8 * af * jMax * tf - 4 * jMax * (jMax * tf_tf - vd)) + 2 * af_af * jMax * (jMax * tf_tf + 2 * vd) - 4 * af * jMax_jMax * (-3 * pd + jMax * tf_p3 + 2 * tf * v0 + tf * vf) + jMax_jMax * (jMax_jMax * tf_p4 - 8 * vd_vd + 4 * jMax * tf * (-3 * pd + tf * v0 + 2 * tf * vf)) + 2 * a0 * (2 * af_p3 - 2 * af_af * jMax * tf + af * jMax * (-3 * jMax * tf_tf - 4 * vd) + jMax_jMax * (-6 * pd + jMax * tf_p3 - 4 * tf * v0 + 10 * tf * vf))) / (jMax_jMax * ph1); + polynom[2] = -(a0_p5 - af_p5 + af_p4 * jMax * tf - 5 * a0_p4 * (af - jMax * tf) + 2 * a0_p3 * ph3 + 4 * af_p3 * jMax * (jMax * tf_tf + vd) + 12 * jMax_jMax * af * ph6 - 2 * a0_a0 * (5 * af_p3 - 9 * af_af * jMax * tf - 6 * af * jMax * vd + 6 * jMax_jMax * (-2 * pd - tf * v0 + 3 * tf * vf)) - 12 * jMax_jMax * jMax * ph2 + a0 * ph5) / (3 * jMax_jMax * jMax * ph1); + polynom[3] = -(-a0_p6 - af_p6 + 6 * a0_p5 * (af - jMax * tf) - 48 * af_p3 * jMax_jMax * g1 + 72 * jMax_jMax * jMax * (jMax * g1 * g1 + vd_vd * vd + 2 * af * g1 * vd) - 3 * a0_p4 * ph3 - 36 * af_af * jMax_jMax * vd_vd + 6 * af_p4 * jMax * vd + 4 * a0_p3 * (5 * af_p3 - 9 * af_af * jMax * tf - 6 * af * jMax * vd + 6 * jMax_jMax * (-2 * pd - tf * v0 + 3 * tf * vf)) - 3 * a0_a0 * ph5 + 6 * a0 * (af_p5 - af_p4 * jMax * tf - 4 * af_p3 * jMax * (jMax * tf_tf + vd) + 12 * jMax_jMax * (-af * ph6 + jMax * ph2))) / (18 * jMax_jMax * jMax_jMax * ph1); + + const double t_max_v = (a0 - aMin) / jMax; + + CRuckigRootSet roots = cruckig_roots_solve_quart_monic(polynom[0], polynom[1], polynom[2], polynom[3]); + cruckig_root_set_sort(&roots); + for (size_t i = 0; i < roots.size; ++i) { + double t = roots.data[i]; + if (t > t_max_v) { + continue; + } + + /* Single Newton step (regarding pd) */ + { + const double h1_v = ad_ad / 2 + jMax * (af * t + (jMax * t - a0) * (t - tf) - vd); + const double h2_v = -ad + jMax * (tf - 2 * t); + const double h3 = sqrt(h1_v); + const double orig = (af_p3 - a0_p3 + 3 * af * jMax * t * (af + jMax * t) + 3 * a0_a0 * (af + jMax * t) - 3 * a0 * (af_af + 2 * af * jMax * t + jMax_jMax * (t * t - tf_tf)) + 3 * jMax_jMax * (-2 * pd + jMax * t * (t - tf) * tf + 2 * tf * v0)) / (6 * jMax_jMax) - h3 * h3 * h3 / (jMax * fabs(jMax)) + ((-ad - jMax * t) * h1_v) / (jMax_jMax); + const double deriv_n = (6 * jMax * h2_v * h3 / fabs(jMax) + 2 * (-ad - jMax * tf) * h2_v - 2 * (3 * ad_ad + af * jMax * (8 * t - 2 * tf) + 4 * a0 * jMax * (-2 * t + tf) + 2 * jMax * (jMax * t * (3 * t - 2 * tf) - vd))) / (4 * jMax); + + t -= orig / deriv_n; + } + + { + const double h1_v = sqrt(2 * ad_ad + 4 * jMax * (ad * t + a0 * tf + jMax * t * (t - tf) - vd)) / fabs(jMax); + + /* Solution 2 with aPlat */ + profile->t[0] = 0; + profile->t[1] = 0; + profile->t[2] = t; + profile->t[3] = tf - 2 * t - ad / jMax - h1_v; + profile->t[4] = h1_v / 2; + profile->t[5] = 0; + profile->t[6] = tf - (t + profile->t[3] + profile->t[4]); + + if (cruckig_profile_check_with_timing(profile, ControlSignsUDDU, ReachedLimitsNONE, tf, jMax, vMax, vMin, aMax, aMin)) { + return true; + } + } + } + } + } + + /* Profiles with a3 != 0, Solution UDUD */ + { + /* T 0124 */ + { + const double ph0 = -2 * pd - tf * v0 + 3 * tf * vf; + const double ph1 = -ad + jMax * tf; + const double ph2 = jMax * tf_tf * g1 - vd * ph0; + const double ph3 = 5 * af_af + 2 * jMax * (2 * jMax * tf_tf - vd - 4 * af * tf); + const double ph4 = jMax_jMax * tf_p4 - 2 * vd_vd + 8 * jMax * tf * (-pd + tf * vf); + const double ph5 = (5 * af_p4 - 8 * af_p3 * jMax * tf - 12 * af_af * jMax * (jMax * tf_tf + vd) + 24 * af * jMax_jMax * (-2 * pd + jMax * tf_p3 + 2 * tf * vf) - 6 * jMax_jMax * ph4); + const double ph6 = -vd_vd + jMax * tf * (-2 * pd + 3 * tf * v0 - tf * vf); + const double ph7 = 3 * jMax_jMax * ph1 * ph1; + + double polynom[4]; + polynom[0] = (4 * af * tf - 2 * jMax * tf_tf - 4 * vd) / ph1; + polynom[1] = (-2 * (a0_p4 + af_p4) + 8 * af_p3 * jMax * tf + 6 * af_af * jMax_jMax * tf_tf + 8 * a0_p3 * (af - jMax * tf) - 12 * a0_a0 * (af - jMax * tf) * (af - jMax * tf) - 12 * af * jMax_jMax * (-pd + jMax * tf_p3 - 2 * tf * v0 + 3 * tf * vf) + 2 * a0 * (4 * af_p3 - 12 * af_af * jMax * tf + 9 * af * jMax_jMax * tf_tf - 3 * jMax_jMax * (2 * pd + jMax * tf_p3 - 2 * tf * vf)) + 3 * jMax_jMax * (jMax_jMax * tf_p4 + 4 * vd_vd - 4 * jMax * tf * (pd + tf * v0 - 2 * tf * vf))) / ph7; + polynom[2] = (-a0_p5 + af_p5 - af_p4 * jMax * tf + 5 * a0_p4 * (af - jMax * tf) - 2 * a0_p3 * ph3 - 4 * af_p3 * jMax * (jMax * tf_tf + vd) + 12 * af_af * jMax_jMax * g2 - 12 * af * jMax_jMax * ph6 + 2 * a0_a0 * (5 * af_p3 - 9 * af_af * jMax * tf - 6 * af * jMax * vd + 6 * jMax_jMax * ph0) + 12 * jMax_jMax * jMax * ph2 + a0 * (-5 * af_p4 + 8 * af_p3 * jMax * tf + 12 * af_af * jMax * (jMax * tf_tf + vd) - 24 * af * jMax_jMax * (-2 * pd + jMax * tf_p3 + 2 * tf * vf) + 6 * jMax_jMax * ph4)) / (jMax * ph7); + polynom[3] = -(a0_p6 + af_p6 - 6 * a0_p5 * (af - jMax * tf) + 48 * af_p3 * jMax_jMax * g1 - 72 * jMax_jMax * jMax * (jMax * g1 * g1 + vd_vd * vd + 2 * af * g1 * vd) + 3 * a0_p4 * ph3 - 6 * af_p4 * jMax * vd + 36 * af_af * jMax_jMax * vd_vd - 4 * a0_p3 * (5 * af_p3 - 9 * af_af * jMax * tf - 6 * af * jMax * vd + 6 * jMax_jMax * ph0) + 3 * a0_a0 * ph5 - 6 * a0 * (af_p5 - af_p4 * jMax * tf - 4 * af_p3 * jMax * (jMax * tf_tf + vd) + 12 * jMax_jMax * (af_af * g2 - af * ph6 + jMax * ph2))) / (6 * jMax_jMax * ph7); + + CRuckigRootSet roots = cruckig_roots_solve_quart_monic(polynom[0], polynom[1], polynom[2], polynom[3]); + cruckig_root_set_sort(&roots); + for (size_t i = 0; i < roots.size; ++i) { + double t = roots.data[i]; + if (t > tf || t > (aMax - a0) / jMax) { + continue; + } + + { + const double h1_v = sqrt(ad_ad / (2 * jMax_jMax) + (a0 * (t + tf) - af * t + jMax * t * tf - vd) / jMax); + + profile->t[0] = t; + profile->t[1] = tf - ad / jMax - 2 * h1_v; + profile->t[2] = h1_v; + profile->t[3] = 0; + profile->t[4] = ad / jMax + h1_v - t; + profile->t[5] = 0; + profile->t[6] = 0; + + if (cruckig_profile_check_with_timing(profile, ControlSignsUDUD, ReachedLimitsNONE, tf, jMax, vMax, vMin, aMax, aMin)) { + return true; + } + } + } + } + } + + /* 3 step profile (ak. UZD), sometimes missed because of numerical errors T 012 */ + { + const double h1 = sqrt(-ad_ad + jMax * (2 * (a0 + af) * tf - 4 * vd + jMax * tf_tf)) / fabs(jMax); + + profile->t[0] = (tf - h1 + ad / jMax) / 2; + profile->t[1] = h1; + profile->t[2] = (tf - h1 - ad / jMax) / 2; + profile->t[3] = 0; + profile->t[4] = 0; + profile->t[5] = 0; + profile->t[6] = 0; + + if (cruckig_profile_check_with_timing(profile, ControlSignsUDDU, ReachedLimitsNONE, tf, jMax, vMax, vMin, aMax, aMin)) { + return true; + } + } + + /* 3 step profile (ak. UZU), sometimes missed because of numerical errors */ + { + double polynom[4]; + polynom[0] = ad_ad; + polynom[1] = ad_ad * tf; + polynom[2] = (a0_a0 + af_af + 10 * a0 * af) * tf_tf + 24 * (tf * (af * v0 - a0 * vf) - pd * ad) + 12 * vd_vd; + polynom[3] = -3 * tf * ((a0_a0 + af_af + 2 * a0 * af) * tf_tf - 4 * vd * (a0 + af) * tf + 4 * vd_vd); + + CRuckigRootSet roots = cruckig_roots_solve_cubic(polynom[0], polynom[1], polynom[2], polynom[3]); + cruckig_root_set_sort(&roots); + for (size_t i = 0; i < roots.size; ++i) { + double t = roots.data[i]; + if (t > tf) { + continue; + } + + const double jf = ad / (tf - t); + + profile->t[0] = (2 * (vd - a0 * tf) + ad * (t - tf)) / (2 * jf * t); + profile->t[1] = t; + profile->t[2] = 0; + profile->t[3] = 0; + profile->t[4] = 0; + profile->t[5] = 0; + profile->t[6] = tf - (profile->t[0] + profile->t[1]); + + if (cruckig_profile_check_with_timing_full(profile, ControlSignsUDDU, ReachedLimitsNONE, tf, jf, vMax, vMin, aMax, aMin, jMax)) { + return true; + } + } + } + + /* 3 step profile (ak. UDU), sometimes missed because of numerical errors */ + { + profile->t[0] = (ad_ad / jMax + 2 * (a0 + af) * tf - jMax * tf_tf - 4 * vd) / (4 * (ad - jMax * tf)); + profile->t[1] = 0; + profile->t[2] = -ad / (2 * jMax) + tf / 2; + profile->t[3] = 0; + profile->t[4] = 0; + profile->t[5] = 0; + profile->t[6] = tf - (profile->t[0] + profile->t[2]); + + if (cruckig_profile_check_with_timing(profile, ControlSignsUDDU, ReachedLimitsNONE, tf, jMax, vMax, vMin, aMax, aMin)) { + return true; + } + } + + return false; +} + +CRUCKIG_HOT +bool cruckig_pos3_step2_get_profile(CRuckigPositionThirdOrderStep2 *s, CRuckigProfile *profile) +{ + /* Test all cases to get ones that match */ + const bool up_first = (s->pd > s->tf * s->v0); + const double vMax = up_first ? s->_vMax : s->_vMin; + const double vMin = up_first ? s->_vMin : s->_vMax; + const double aMax = up_first ? s->_aMax : s->_aMin; + const double aMin = up_first ? s->_aMin : s->_aMax; + const double jMax = up_first ? s->_jMax : -s->_jMax; + + return time_acc0_acc1_vel(s, profile, vMax, vMin, aMax, aMin, jMax) + || time_vel(s, profile, vMax, vMin, aMax, aMin, jMax) + || time_acc0_vel(s, profile, vMax, vMin, aMax, aMin, jMax) + || time_acc1_vel(s, profile, vMax, vMin, aMax, aMin, jMax) + || time_acc0_acc1_vel(s, profile, vMin, vMax, aMin, aMax, -jMax) + || time_vel(s, profile, vMin, vMax, aMin, aMax, -jMax) + || time_acc0_vel(s, profile, vMin, vMax, aMin, aMax, -jMax) + || time_acc1_vel(s, profile, vMin, vMax, aMin, aMax, -jMax) + || time_acc0_acc1(s, profile, vMax, vMin, aMax, aMin, jMax) + || time_acc0(s, profile, vMax, vMin, aMax, aMin, jMax) + || time_acc1(s, profile, vMax, vMin, aMax, aMin, jMax) + || time_none(s, profile, vMax, vMin, aMax, aMin, jMax) + || time_acc0_acc1(s, profile, vMin, vMax, aMin, aMax, -jMax) + || time_acc0(s, profile, vMin, vMax, aMin, aMax, -jMax) + || time_acc1(s, profile, vMin, vMax, aMin, aMax, -jMax) + || time_none(s, profile, vMin, vMax, aMin, aMax, -jMax); +} diff --git a/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/profile.c b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/profile.c new file mode 100644 index 0000000..781292a --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/profile.c @@ -0,0 +1,539 @@ +/* + * cruckig - Pure C99 port of the Ruckig trajectory generation library + * Copyright (c) 2025 Yang Yang + * Copyright (c) 2021 Lars Berscheid (original C++ Ruckig) + * + * License: MIT, see the LICENSE file in this directory. + */ + +#include "profile.h" +#include "roots.h" +#include "utils.h" + +void cruckig_profile_init(CRuckigProfile *p) { + memset(p->t, 0, sizeof(p->t)); + memset(p->t_sum, 0, sizeof(p->t_sum)); + memset(p->j, 0, sizeof(p->j)); + memset(p->a, 0, sizeof(p->a)); + memset(p->v, 0, sizeof(p->v)); + memset(p->p, 0, sizeof(p->p)); + + cruckig_brake_init(&p->brake); + cruckig_brake_init(&p->accel); + + p->pf = 0.0; + p->vf = 0.0; + p->af = 0.0; + + p->limits = ReachedLimitsNONE; + p->direction = DirectionUP; + p->control_signs = ControlSignsUDDU; +} + +void cruckig_profile_set_boundary(CRuckigProfile *p, double p0, double v0, double a0, + double pf, double vf, double af) { + p->a[0] = a0; + p->v[0] = v0; + p->p[0] = p0; + p->af = af; + p->vf = vf; + p->pf = pf; +} + +void cruckig_profile_set_boundary_from_profile(CRuckigProfile *p, const CRuckigProfile *src) { + p->a[0] = src->a[0]; + p->v[0] = src->v[0]; + p->p[0] = src->p[0]; + p->af = src->af; + p->vf = src->vf; + p->pf = src->pf; + p->brake = src->brake; + p->accel = src->accel; +} + +void cruckig_profile_set_boundary_for_velocity(CRuckigProfile *p, double p0, double v0, double a0, + double vf, double af) { + p->a[0] = a0; + p->v[0] = v0; + p->p[0] = p0; + p->af = af; + p->vf = vf; +} + +/* Third-order position check */ +CRUCKIG_HOT +bool cruckig_profile_check(CRuckigProfile *p, CRuckigControlSigns cs, CRuckigReachedLimits lim, + bool set_limits, double jf, double vMax, double vMin, double aMax, double aMin) { + if (CRUCKIG_UNLIKELY(p->t[0] < 0)) { + return false; + } + + p->t_sum[0] = p->t[0]; + for (size_t i = 0; i < 6; ++i) { + if (CRUCKIG_UNLIKELY(p->t[i + 1] < 0)) { + return false; + } + p->t_sum[i + 1] = p->t_sum[i] + p->t[i + 1]; + } + + if (lim == ReachedLimitsACC0_ACC1_VEL || lim == ReachedLimitsACC0_VEL || lim == ReachedLimitsACC1_VEL || lim == ReachedLimitsVEL) { + if (CRUCKIG_UNLIKELY(p->t[3] < DBL_EPSILON)) { + return false; + } + } + + if (lim == ReachedLimitsACC0 || lim == ReachedLimitsACC0_ACC1) { + if (CRUCKIG_UNLIKELY(p->t[1] < DBL_EPSILON)) { + return false; + } + } + + if (lim == ReachedLimitsACC1 || lim == ReachedLimitsACC0_ACC1) { + if (CRUCKIG_UNLIKELY(p->t[5] < DBL_EPSILON)) { + return false; + } + } + + if (CRUCKIG_UNLIKELY(p->t_sum[6] > PROFILE_T_MAX)) { + return false; + } + + if (cs == ControlSignsUDDU) { + p->j[0] = (p->t[0] > 0 ? jf : 0); + p->j[1] = 0; + p->j[2] = (p->t[2] > 0 ? -jf : 0); + p->j[3] = 0; + p->j[4] = (p->t[4] > 0 ? -jf : 0); + p->j[5] = 0; + p->j[6] = (p->t[6] > 0 ? jf : 0); + } else { + p->j[0] = (p->t[0] > 0 ? jf : 0); + p->j[1] = 0; + p->j[2] = (p->t[2] > 0 ? -jf : 0); + p->j[3] = 0; + p->j[4] = (p->t[4] > 0 ? jf : 0); + p->j[5] = 0; + p->j[6] = (p->t[6] > 0 ? -jf : 0); + } + + p->direction = (vMax > 0) ? DirectionUP : DirectionDOWN; + const double vUppLim = (p->direction == DirectionUP ? vMax : vMin) + PROFILE_V_EPS; + const double vLowLim = (p->direction == DirectionUP ? vMin : vMax) - PROFILE_V_EPS; + + for (size_t i = 0; i < 7; ++i) { + p->a[i + 1] = p->a[i] + p->t[i] * p->j[i]; + p->v[i + 1] = p->v[i] + p->t[i] * (p->a[i] + p->t[i] * p->j[i] / 2); + p->p[i + 1] = p->p[i] + p->t[i] * (p->v[i] + p->t[i] * (p->a[i] / 2 + p->t[i] * p->j[i] / 6)); + + if (lim == ReachedLimitsACC0_ACC1_VEL || lim == ReachedLimitsACC0_ACC1 || lim == ReachedLimitsACC0_VEL || lim == ReachedLimitsACC1_VEL || lim == ReachedLimitsVEL) { + if (i == 2) { + p->a[3] = 0.0; + } + } + + if (set_limits) { + if (lim == ReachedLimitsACC1) { + if (i == 2) { + p->a[3] = aMin; + } + } + + if (lim == ReachedLimitsACC0_ACC1) { + if (i == 0) { + p->a[1] = aMax; + } + if (i == 4) { + p->a[5] = aMin; + } + } + } + + if (i > 1 && p->a[i + 1] * p->a[i] < -DBL_EPSILON) { + const double v_a_zero = p->v[i] - (p->a[i] * p->a[i]) / (2 * p->j[i]); + if (v_a_zero > vUppLim || v_a_zero < vLowLim) { + return false; + } + } + } + + p->control_signs = cs; + p->limits = lim; + + const double aUppLim = (p->direction == DirectionUP ? aMax : aMin) + PROFILE_A_EPS; + const double aLowLim = (p->direction == DirectionUP ? aMin : aMax) - PROFILE_A_EPS; + + return fabs(p->p[7] - p->pf) < PROFILE_P_PREC && fabs(p->v[7] - p->vf) < PROFILE_V_PREC && fabs(p->a[7] - p->af) < PROFILE_A_PREC + && p->a[1] >= aLowLim && p->a[3] >= aLowLim && p->a[5] >= aLowLim + && p->a[1] <= aUppLim && p->a[3] <= aUppLim && p->a[5] <= aUppLim + && p->v[3] <= vUppLim && p->v[4] <= vUppLim && p->v[5] <= vUppLim && p->v[6] <= vUppLim + && p->v[3] >= vLowLim && p->v[4] >= vLowLim && p->v[5] >= vLowLim && p->v[6] >= vLowLim; +} + +bool cruckig_profile_check_with_timing(CRuckigProfile *p, CRuckigControlSigns cs, CRuckigReachedLimits lim, + double tf, double jf, double vMax, double vMin, double aMax, double aMin) { + (void)tf; + /* Time doesn't need to be checked as every profile has a: tf - ... equation */ + return cruckig_profile_check(p, cs, lim, false, jf, vMax, vMin, aMax, aMin); +} + +bool cruckig_profile_check_with_timing_full(CRuckigProfile *p, CRuckigControlSigns cs, CRuckigReachedLimits lim, + double tf, double jf, double vMax, double vMin, double aMax, double aMin, double jMax) { + return (fabs(jf) < fabs(jMax) + PROFILE_J_EPS) && cruckig_profile_check_with_timing(p, cs, lim, tf, jf, vMax, vMin, aMax, aMin); +} + +/* Third-order velocity check */ +CRUCKIG_HOT +bool cruckig_profile_check_for_velocity(CRuckigProfile *p, CRuckigControlSigns cs, CRuckigReachedLimits lim, + double jf, double aMax, double aMin) { + if (CRUCKIG_UNLIKELY(p->t[0] < 0)) { + return false; + } + + p->t_sum[0] = p->t[0]; + for (size_t i = 0; i < 6; ++i) { + if (p->t[i + 1] < 0) { + return false; + } + p->t_sum[i + 1] = p->t_sum[i] + p->t[i + 1]; + } + + if (lim == ReachedLimitsACC0) { + if (p->t[1] < DBL_EPSILON) { + return false; + } + } + + if (p->t_sum[6] > PROFILE_T_MAX) { + return false; + } + + if (cs == ControlSignsUDDU) { + p->j[0] = (p->t[0] > 0 ? jf : 0); + p->j[1] = 0; + p->j[2] = (p->t[2] > 0 ? -jf : 0); + p->j[3] = 0; + p->j[4] = (p->t[4] > 0 ? -jf : 0); + p->j[5] = 0; + p->j[6] = (p->t[6] > 0 ? jf : 0); + } else { + p->j[0] = (p->t[0] > 0 ? jf : 0); + p->j[1] = 0; + p->j[2] = (p->t[2] > 0 ? -jf : 0); + p->j[3] = 0; + p->j[4] = (p->t[4] > 0 ? jf : 0); + p->j[5] = 0; + p->j[6] = (p->t[6] > 0 ? -jf : 0); + } + + for (size_t i = 0; i < 7; ++i) { + p->a[i + 1] = p->a[i] + p->t[i] * p->j[i]; + p->v[i + 1] = p->v[i] + p->t[i] * (p->a[i] + p->t[i] * p->j[i] / 2); + p->p[i + 1] = p->p[i] + p->t[i] * (p->v[i] + p->t[i] * (p->a[i] / 2 + p->t[i] * p->j[i] / 6)); + } + + p->control_signs = cs; + p->limits = lim; + + p->direction = (aMax > 0) ? DirectionUP : DirectionDOWN; + const double aUppLim = (p->direction == DirectionUP ? aMax : aMin) + PROFILE_A_EPS; + const double aLowLim = (p->direction == DirectionUP ? aMin : aMax) - PROFILE_A_EPS; + + return fabs(p->v[7] - p->vf) < PROFILE_V_PREC && fabs(p->a[7] - p->af) < PROFILE_A_PREC + && p->a[1] >= aLowLim && p->a[3] >= aLowLim && p->a[5] >= aLowLim + && p->a[1] <= aUppLim && p->a[3] <= aUppLim && p->a[5] <= aUppLim; +} + +bool cruckig_profile_check_for_velocity_with_timing(CRuckigProfile *p, CRuckigControlSigns cs, CRuckigReachedLimits lim, + double tf, double jf, double aMax, double aMin) { + (void)tf; + return cruckig_profile_check_for_velocity(p, cs, lim, jf, aMax, aMin); +} + +bool cruckig_profile_check_for_velocity_with_timing_full(CRuckigProfile *p, CRuckigControlSigns cs, CRuckigReachedLimits lim, + double tf, double jf, double aMax, double aMin, double jMax) { + return (fabs(jf) < fabs(jMax) + PROFILE_J_EPS) && cruckig_profile_check_for_velocity_with_timing(p, cs, lim, tf, jf, aMax, aMin); +} + +/* Second-order position check */ +bool cruckig_profile_check_for_second_order(CRuckigProfile *p, CRuckigControlSigns cs, CRuckigReachedLimits lim, + double aUp, double aDown, double vMax, double vMin) { + if (p->t[0] < 0) { + return false; + } + + p->t_sum[0] = p->t[0]; + for (size_t i = 0; i < 6; ++i) { + if (p->t[i + 1] < 0) { + return false; + } + p->t_sum[i + 1] = p->t_sum[i] + p->t[i + 1]; + } + + if (p->t_sum[6] > PROFILE_T_MAX) { + return false; + } + + p->j[0] = 0; p->j[1] = 0; p->j[2] = 0; p->j[3] = 0; + p->j[4] = 0; p->j[5] = 0; p->j[6] = 0; + + if (cs == ControlSignsUDDU) { + p->a[0] = (p->t[0] > 0 ? aUp : 0); + p->a[1] = 0; + p->a[2] = (p->t[2] > 0 ? aDown : 0); + p->a[3] = 0; + p->a[4] = (p->t[4] > 0 ? aDown : 0); + p->a[5] = 0; + p->a[6] = (p->t[6] > 0 ? aUp : 0); + p->a[7] = p->af; + } else { + p->a[0] = (p->t[0] > 0 ? aUp : 0); + p->a[1] = 0; + p->a[2] = (p->t[2] > 0 ? aDown : 0); + p->a[3] = 0; + p->a[4] = (p->t[4] > 0 ? aUp : 0); + p->a[5] = 0; + p->a[6] = (p->t[6] > 0 ? aDown : 0); + p->a[7] = p->af; + } + + p->direction = (vMax > 0) ? DirectionUP : DirectionDOWN; + const double vUppLim = (p->direction == DirectionUP ? vMax : vMin) + PROFILE_V_EPS; + const double vLowLim = (p->direction == DirectionUP ? vMin : vMax) - PROFILE_V_EPS; + + for (size_t i = 0; i < 7; ++i) { + p->v[i + 1] = p->v[i] + p->t[i] * p->a[i]; + p->p[i + 1] = p->p[i] + p->t[i] * (p->v[i] + p->t[i] * p->a[i] / 2); + } + + p->control_signs = cs; + p->limits = lim; + + return fabs(p->p[7] - p->pf) < PROFILE_P_PREC && fabs(p->v[7] - p->vf) < PROFILE_V_PREC + && p->v[2] <= vUppLim && p->v[3] <= vUppLim && p->v[4] <= vUppLim && p->v[5] <= vUppLim && p->v[6] <= vUppLim + && p->v[2] >= vLowLim && p->v[3] >= vLowLim && p->v[4] >= vLowLim && p->v[5] >= vLowLim && p->v[6] >= vLowLim; +} + +bool cruckig_profile_check_for_second_order_with_timing(CRuckigProfile *p, CRuckigControlSigns cs, CRuckigReachedLimits lim, + double tf, double aUp, double aDown, double vMax, double vMin) { + (void)tf; + return cruckig_profile_check_for_second_order(p, cs, lim, aUp, aDown, vMax, vMin); +} + +bool cruckig_profile_check_for_second_order_with_timing_full(CRuckigProfile *p, CRuckigControlSigns cs, CRuckigReachedLimits lim, + double tf, double aUp, double aDown, double vMax, double vMin, + double aMax, double aMin) { + return (aMin - PROFILE_A_EPS < aUp) && (aUp < aMax + PROFILE_A_EPS) && (aMin - PROFILE_A_EPS < aDown) && (aDown < aMax + PROFILE_A_EPS) + && cruckig_profile_check_for_second_order_with_timing(p, cs, lim, tf, aUp, aDown, vMax, vMin); +} + +/* Second-order velocity check */ +bool cruckig_profile_check_for_second_order_velocity(CRuckigProfile *p, CRuckigControlSigns cs, CRuckigReachedLimits lim, + double aUp) { + /* ReachedLimits::ACC0 */ + if (p->t[1] < 0.0) { + return false; + } + + p->t_sum[0] = 0; + p->t_sum[1] = p->t[1]; + p->t_sum[2] = p->t[1]; + p->t_sum[3] = p->t[1]; + p->t_sum[4] = p->t[1]; + p->t_sum[5] = p->t[1]; + p->t_sum[6] = p->t[1]; + + if (p->t_sum[6] > PROFILE_T_MAX) { + return false; + } + + p->j[0] = 0; p->j[1] = 0; p->j[2] = 0; p->j[3] = 0; + p->j[4] = 0; p->j[5] = 0; p->j[6] = 0; + + p->a[0] = 0; + p->a[1] = (p->t[1] > 0) ? aUp : 0; + p->a[2] = 0; p->a[3] = 0; p->a[4] = 0; p->a[5] = 0; p->a[6] = 0; + p->a[7] = p->af; + + for (size_t i = 0; i < 7; ++i) { + p->v[i + 1] = p->v[i] + p->t[i] * p->a[i]; + p->p[i + 1] = p->p[i] + p->t[i] * (p->v[i] + p->t[i] * p->a[i] / 2); + } + + p->control_signs = cs; + p->limits = lim; + + p->direction = (aUp > 0) ? DirectionUP : DirectionDOWN; + + return fabs(p->v[7] - p->vf) < PROFILE_V_PREC; +} + +bool cruckig_profile_check_for_second_order_velocity_with_timing(CRuckigProfile *p, CRuckigControlSigns cs, CRuckigReachedLimits lim, + double tf, double aUp) { + (void)tf; + return cruckig_profile_check_for_second_order_velocity(p, cs, lim, aUp); +} + +bool cruckig_profile_check_for_second_order_velocity_with_timing_full(CRuckigProfile *p, CRuckigControlSigns cs, CRuckigReachedLimits lim, + double tf, double aUp, double aMax, double aMin) { + return (aMin - PROFILE_A_EPS < aUp) && (aUp < aMax + PROFILE_A_EPS) + && cruckig_profile_check_for_second_order_velocity_with_timing(p, cs, lim, tf, aUp); +} + +/* First-order position check */ +bool cruckig_profile_check_for_first_order(CRuckigProfile *p, CRuckigControlSigns cs, CRuckigReachedLimits lim, + double vUp) { + /* ReachedLimits::VEL */ + if (p->t[3] < 0.0) { + return false; + } + + p->t_sum[0] = 0; p->t_sum[1] = 0; p->t_sum[2] = 0; + p->t_sum[3] = p->t[3]; + p->t_sum[4] = p->t[3]; p->t_sum[5] = p->t[3]; p->t_sum[6] = p->t[3]; + + if (p->t_sum[6] > PROFILE_T_MAX) { + return false; + } + + p->j[0] = 0; p->j[1] = 0; p->j[2] = 0; p->j[3] = 0; + p->j[4] = 0; p->j[5] = 0; p->j[6] = 0; + + p->a[0] = 0; p->a[1] = 0; p->a[2] = 0; p->a[3] = 0; + p->a[4] = 0; p->a[5] = 0; p->a[6] = 0; p->a[7] = p->af; + + p->v[0] = 0; p->v[1] = 0; p->v[2] = 0; + p->v[3] = (p->t[3] > 0 ? vUp : 0); + p->v[4] = 0; p->v[5] = 0; p->v[6] = 0; p->v[7] = p->vf; + + for (size_t i = 0; i < 7; ++i) { + p->p[i + 1] = p->p[i] + p->t[i] * (p->v[i] + p->t[i] * p->a[i] / 2); + } + + p->control_signs = cs; + p->limits = lim; + + p->direction = (vUp > 0) ? DirectionUP : DirectionDOWN; + + return fabs(p->p[7] - p->pf) < PROFILE_P_PREC; +} + +bool cruckig_profile_check_for_first_order_with_timing(CRuckigProfile *p, CRuckigControlSigns cs, CRuckigReachedLimits lim, + double tf, double vUp) { + (void)tf; + return cruckig_profile_check_for_first_order(p, cs, lim, vUp); +} + +bool cruckig_profile_check_for_first_order_with_timing_full(CRuckigProfile *p, CRuckigControlSigns cs, CRuckigReachedLimits lim, + double tf, double vUp, double vMax, double vMin) { + return (vMin - PROFILE_V_EPS < vUp) && (vUp < vMax + PROFILE_V_EPS) + && cruckig_profile_check_for_first_order_with_timing(p, cs, lim, tf, vUp); +} + +/* Position extrema helpers */ +static void check_position_extremum(double t_ext, double t_sum_val, double t_seg, double pos, double vel, double acc, double jrk, CRuckigBound *ext) { + if (0 < t_ext && t_ext < t_seg) { + double p_ext, v_ext, a_ext; + cruckig_integrate(t_ext, pos, vel, acc, jrk, &p_ext, &v_ext, &a_ext); + (void)v_ext; + if (a_ext > 0 && p_ext < ext->min) { + ext->min = p_ext; + ext->t_min = t_sum_val + t_ext; + } else if (a_ext < 0 && p_ext > ext->max) { + ext->max = p_ext; + ext->t_max = t_sum_val + t_ext; + } + } +} + +static void check_step_for_position_extremum(double t_sum_val, double t_seg, double pos, double vel, double acc, double jrk, CRuckigBound *ext) { + if (pos < ext->min) { + ext->min = pos; + ext->t_min = t_sum_val; + } + if (pos > ext->max) { + ext->max = pos; + ext->t_max = t_sum_val; + } + + if (jrk != 0) { + const double D = acc * acc - 2 * jrk * vel; + if (fabs(D) < DBL_EPSILON) { + check_position_extremum(-acc / jrk, t_sum_val, t_seg, pos, vel, acc, jrk, ext); + } else if (D > 0.0) { + const double D_sqrt = sqrt(D); + check_position_extremum((-acc - D_sqrt) / jrk, t_sum_val, t_seg, pos, vel, acc, jrk, ext); + check_position_extremum((-acc + D_sqrt) / jrk, t_sum_val, t_seg, pos, vel, acc, jrk, ext); + } + } +} + +CRuckigBound cruckig_profile_get_position_extrema(const CRuckigProfile *p) { + CRuckigBound extrema; + extrema.min = INFINITY; + extrema.max = -INFINITY; + extrema.t_min = 0.0; + extrema.t_max = 0.0; + + if (p->brake.duration > 0.0) { + if (p->brake.t[0] > 0.0) { + check_step_for_position_extremum(0.0, p->brake.t[0], p->brake.p[0], p->brake.v[0], p->brake.a[0], p->brake.j[0], &extrema); + + if (p->brake.t[1] > 0.0) { + check_step_for_position_extremum(p->brake.t[0], p->brake.t[1], p->brake.p[1], p->brake.v[1], p->brake.a[1], p->brake.j[1], &extrema); + } + } + } + + double t_current_sum = 0.0; + for (size_t i = 0; i < 7; ++i) { + if (i > 0) { + t_current_sum = p->t_sum[i - 1]; + } + check_step_for_position_extremum(t_current_sum + p->brake.duration, p->t[i], p->p[i], p->v[i], p->a[i], p->j[i], &extrema); + } + + if (p->pf < extrema.min) { + extrema.min = p->pf; + extrema.t_min = p->t_sum[6] + p->brake.duration; + } + if (p->pf > extrema.max) { + extrema.max = p->pf; + extrema.t_max = p->t_sum[6] + p->brake.duration; + } + + return extrema; +} + +bool cruckig_profile_get_first_state_at_position(const CRuckigProfile *p, double pt, double *time, double time_after) { + double t_cum = 0.0; + + for (size_t i = 0; i < 7; ++i) { + if (p->t[i] == 0.0) { + continue; + } + + if (fabs(p->p[i] - pt) < DBL_EPSILON && t_cum >= time_after) { + *time = t_cum; + return true; + } + + CRuckigRootSet cubic_roots = cruckig_roots_solve_cubic(p->j[i] / 6, p->a[i] / 2, p->v[i], p->p[i] - pt); + cruckig_root_set_sort(&cubic_roots); + for (size_t r = 0; r < cubic_roots.size; ++r) { + double _t = cubic_roots.data[r]; + if (0 < _t && time_after - t_cum <= _t && _t <= p->t[i]) { + *time = _t + t_cum; + return true; + } + } + + t_cum += p->t[i]; + } + + if ((p->t[6] > 0.0 || p->t_sum[6] == 0.0) && fabs(p->pf - pt) < 1e-9 && p->t_sum[6] >= time_after) { + *time = p->t_sum[6]; + return true; + } + + return false; +} diff --git a/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/profile.h b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/profile.h new file mode 100644 index 0000000..212f085 --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/profile.h @@ -0,0 +1,126 @@ +/* + * cruckig - Pure C99 port of the Ruckig trajectory generation library + * Copyright (c) 2025 Yang Yang + * Copyright (c) 2021 Lars Berscheid (original C++ Ruckig) + * + * License: MIT, see the LICENSE file in this directory. + */ +#ifndef CRUCKIG_PROFILE_H +#define CRUCKIG_PROFILE_H + +#include "cruckig_internal.h" +#include "brake.h" + +/* Constants */ +#define PROFILE_V_EPS 1e-12 +#define PROFILE_A_EPS 1e-12 +#define PROFILE_J_EPS 1e-12 +#define PROFILE_P_PREC 1e-8 +#define PROFILE_V_PREC 1e-8 +#define PROFILE_A_PREC 1e-10 +#define PROFILE_T_PREC 1e-12 +#define PROFILE_T_MAX 1e12 + +typedef enum { + ReachedLimitsACC0_ACC1_VEL = 0, + ReachedLimitsVEL, + ReachedLimitsACC0, + ReachedLimitsACC1, + ReachedLimitsACC0_ACC1, + ReachedLimitsACC0_VEL, + ReachedLimitsACC1_VEL, + ReachedLimitsNONE +} CRuckigReachedLimits; + +typedef enum { + DirectionUP = 0, + DirectionDOWN +} CRuckigDirection; + +typedef enum { + ControlSignsUDDU = 0, + ControlSignsUDUD +} CRuckigControlSigns; + +/* Position extrema info */ +typedef struct { + double min, max; + double t_min, t_max; +} CRuckigBound; + +/* Single-DOF kinematic profile */ +typedef struct { + double t[7]; + double t_sum[7]; + double j[7]; + double a[8]; + double v[8]; + double p[8]; + + CRuckigBrakeProfile brake; + CRuckigBrakeProfile accel; + + double pf, vf, af; + + CRuckigReachedLimits limits; + CRuckigDirection direction; + CRuckigControlSigns control_signs; +} CRuckigProfile; + +void cruckig_profile_init(CRuckigProfile *p); + +/* Set boundary conditions */ +void cruckig_profile_set_boundary(CRuckigProfile *p, double p0, double v0, double a0, + double pf, double vf, double af); +void cruckig_profile_set_boundary_from_profile(CRuckigProfile *p, const CRuckigProfile *src); +void cruckig_profile_set_boundary_for_velocity(CRuckigProfile *p, double p0, double v0, double a0, + double vf, double af); + +/* Third-order position check */ +bool cruckig_profile_check(CRuckigProfile *p, CRuckigControlSigns cs, CRuckigReachedLimits lim, + bool set_limits, double jf, double vMax, double vMin, double aMax, double aMin); +bool cruckig_profile_check_with_timing(CRuckigProfile *p, CRuckigControlSigns cs, CRuckigReachedLimits lim, + double tf, double jf, double vMax, double vMin, double aMax, double aMin); +bool cruckig_profile_check_with_timing_full(CRuckigProfile *p, CRuckigControlSigns cs, CRuckigReachedLimits lim, + double tf, double jf, double vMax, double vMin, double aMax, double aMin, double jMax); + +/* Third-order velocity check */ +bool cruckig_profile_check_for_velocity(CRuckigProfile *p, CRuckigControlSigns cs, CRuckigReachedLimits lim, + double jf, double aMax, double aMin); +bool cruckig_profile_check_for_velocity_with_timing(CRuckigProfile *p, CRuckigControlSigns cs, CRuckigReachedLimits lim, + double tf, double jf, double aMax, double aMin); +bool cruckig_profile_check_for_velocity_with_timing_full(CRuckigProfile *p, CRuckigControlSigns cs, CRuckigReachedLimits lim, + double tf, double jf, double aMax, double aMin, double jMax); + +/* Second-order position check */ +bool cruckig_profile_check_for_second_order(CRuckigProfile *p, CRuckigControlSigns cs, CRuckigReachedLimits lim, + double aUp, double aDown, double vMax, double vMin); +bool cruckig_profile_check_for_second_order_with_timing(CRuckigProfile *p, CRuckigControlSigns cs, CRuckigReachedLimits lim, + double tf, double aUp, double aDown, double vMax, double vMin); +bool cruckig_profile_check_for_second_order_with_timing_full(CRuckigProfile *p, CRuckigControlSigns cs, CRuckigReachedLimits lim, + double tf, double aUp, double aDown, double vMax, double vMin, + double aMax, double aMin); + +/* Second-order velocity check */ +bool cruckig_profile_check_for_second_order_velocity(CRuckigProfile *p, CRuckigControlSigns cs, CRuckigReachedLimits lim, + double aUp); +bool cruckig_profile_check_for_second_order_velocity_with_timing(CRuckigProfile *p, CRuckigControlSigns cs, CRuckigReachedLimits lim, + double tf, double aUp); +bool cruckig_profile_check_for_second_order_velocity_with_timing_full(CRuckigProfile *p, CRuckigControlSigns cs, CRuckigReachedLimits lim, + double tf, double aUp, double aMax, double aMin); + +/* First-order position check */ +bool cruckig_profile_check_for_first_order(CRuckigProfile *p, CRuckigControlSigns cs, CRuckigReachedLimits lim, + double vUp); +bool cruckig_profile_check_for_first_order_with_timing(CRuckigProfile *p, CRuckigControlSigns cs, CRuckigReachedLimits lim, + double tf, double vUp); +bool cruckig_profile_check_for_first_order_with_timing_full(CRuckigProfile *p, CRuckigControlSigns cs, CRuckigReachedLimits lim, + double tf, double vUp, double vMax, double vMin); + +/* Position extrema */ +CRuckigBound cruckig_profile_get_position_extrema(const CRuckigProfile *p); + +/* First time at position */ +bool cruckig_profile_get_first_state_at_position(const CRuckigProfile *p, double pt, double *time, double time_after); + +#endif /* CRUCKIG_PROFILE_H */ diff --git a/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/result.h b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/result.h new file mode 100644 index 0000000..349fc69 --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/result.h @@ -0,0 +1,40 @@ +/* + * cruckig - Pure C99 port of the Ruckig trajectory generation library + * Copyright (c) 2025 Yang Yang + * Copyright (c) 2021 Lars Berscheid (original C++ Ruckig) + * + * License: MIT, see the LICENSE file in this directory. + */ +#ifndef CRUCKIG_RESULT_H +#define CRUCKIG_RESULT_H + +typedef enum { + CRuckigWorking = 0, + CRuckigFinished = 1, + CRuckigError = -1, + CRuckigErrorInvalidInput = -100, + CRuckigErrorTrajectoryDuration = -101, + CRuckigErrorPositionalLimits = -102, + CRuckigErrorZeroLimits = -104, + CRuckigErrorExecutionTimeCalculation = -110, + CRuckigErrorSynchronizationCalculation = -111 +} CRuckigResult; + +typedef enum { + CRuckigPosition = 0, + CRuckigVelocity = 1 +} CRuckigControlInterface; + +typedef enum { + CRuckigSyncTime = 0, + CRuckigSyncTimeIfNecessary = 1, + CRuckigSyncPhase = 2, + CRuckigSyncNone = 3 +} CRuckigSynchronization; + +typedef enum { + CRuckigContinuous = 0, + CRuckigDiscrete = 1 +} CRuckigDurationDiscretization; + +#endif /* CRUCKIG_RESULT_H */ diff --git a/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/roots.c b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/roots.c new file mode 100644 index 0000000..95a882f --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/roots.c @@ -0,0 +1,408 @@ +/* + * cruckig - Pure C99 port of the Ruckig trajectory generation library + * Copyright (c) 2025 Yang Yang + * Copyright (c) 2021 Lars Berscheid (original C++ Ruckig) + * + * License: MIT, see the LICENSE file in this directory. + */ + +#include "roots.h" + +/* + * cruckig_cbrt() - Cube root, used for cubic/quartic polynomial solving. + * Optimized implementation from musl libc / FreeBSD libmsun. + * Polynomial approximation to 23 bits + one Newton step to 53 bits. + * Error < 0.667 ulps. + * + * Copyright (c) 1993 Sun Microsystems, Inc. All rights reserved. + * Developed at SunPro, a Sun Microsystems, Inc. business. + * Copyright (c) 2005-2020 Rich Felker, et al. (musl libc) + * + * Permission is hereby granted, free of charge, to any person obtaining + * a copy of this software and associated documentation files (the + * "Software"), to deal in the Software without restriction, including + * without limitation the rights to use, copy, modify, merge, publish, + * distribute, sublicense, and/or sell copies of the Software, and to + * permit persons to whom the Software is furnished to do so, subject to + * the following conditions: + * + * The above copyright notice and this permission notice shall be + * included in all copies or substantial portions of the Software. + */ +double cruckig_cbrt(double x) { + static const unsigned B1 = 715094163; + static const unsigned B2 = 696219795; + static const double P0 = 1.87595182427177009643; + static const double P1 = -1.88497979543377169875; + static const double P2 = 1.621429720105354466140; + static const double P3 = -0.758397934778766047437; + static const double P4 = 0.145996192886612446982; + union { double f; unsigned long long i; } u = {x}; + double r, s, t, w; + unsigned hx = u.i >> 32 & 0x7fffffff; + + if (hx >= 0x7ff00000) + return x + x; + + if (hx < 0x00100000) { + u.f = x * 0x1p54; + hx = u.i >> 32 & 0x7fffffff; + if (hx == 0) return x; + hx = hx / 3 + B2; + } else { + hx = hx / 3 + B1; + } + u.i &= 1ULL << 63; + u.i |= (unsigned long long)hx << 32; + t = u.f; + + r = (t * t) * (t / x); + t = t * ((P0 + r * (P1 + r * P2)) + ((r * r) * r) * (P3 + r * P4)); + + u.f = t; + u.i = (u.i + 0x80000000) & 0xffffffffc0000000ULL; + t = u.f; + + s = t * t; + r = x / s; + w = t + t; + r = (r - t) / (w + r); + t = t + t * r; + return t; +} + +void cruckig_root_set_sort(CRuckigRootSet *s) { + /* Insertion sort for small arrays (max 4 elements) */ + for (size_t i = 1; i < s->size; ++i) { + double key = s->data[i]; + size_t j = i; + while (j > 0 && s->data[j - 1] > key) { + s->data[j] = s->data[j - 1]; + --j; + } + s->data[j] = key; + } +} + +CRUCKIG_HOT +CRuckigRootSet cruckig_roots_solve_cubic(double a, double b, double c, double d) { + CRuckigRootSet roots; + cruckig_root_set_init(&roots); + + if (fabs(d) < DBL_EPSILON) { + /* First solution is x = 0 */ + cruckig_root_set_insert(&roots, 0.0); + + /* Converting to a quadratic equation */ + d = c; + c = b; + b = a; + a = 0.0; + } + + if (fabs(a) < DBL_EPSILON) { + if (fabs(b) < DBL_EPSILON) { + /* Linear equation */ + if (fabs(c) > DBL_EPSILON) { + cruckig_root_set_insert(&roots, -d / c); + } + } else { + /* Quadratic equation */ + const double discriminant = c * c - 4 * b * d; + if (discriminant >= 0) { + const double inv2b = 1.0 / (2 * b); + const double y = sqrt(discriminant); + cruckig_root_set_insert(&roots, (-c + y) * inv2b); + cruckig_root_set_insert(&roots, (-c - y) * inv2b); + } + } + } else { + /* Cubic equation */ + const double inva = 1.0 / a; + const double invaa = inva * inva; + const double bb = b * b; + const double bover3a = b * inva / 3; + const double p = (a * c - bb / 3) * invaa; + const double halfq = (2 * bb * b - 9 * a * b * c + 27 * a * a * d) / 54 * invaa * inva; + const double yy = p * p * p / 27 + halfq * halfq; + + const double cos120 = -0.50; + const double sin120 = 0.866025403784438646764; + + if (yy > DBL_EPSILON) { + /* Sqrt is positive: one real solution */ + const double y = sqrt(yy); + const double uuu = -halfq + y; + const double vvv = -halfq - y; + const double www = fabs(uuu) > fabs(vvv) ? uuu : vvv; + const double w = cruckig_cbrt(www); + cruckig_root_set_insert(&roots, w - p / (3 * w) - bover3a); + } else if (yy < -DBL_EPSILON) { + /* Sqrt is negative: three real solutions */ + const double x = -halfq; + const double y = sqrt(-yy); + double theta; + double r; + + /* Convert to polar form */ + if (fabs(x) > DBL_EPSILON) { + theta = (x > 0.0) ? atan(y / x) : (atan(y / x) + M_PI); + r = sqrt(x * x - yy); + } else { + /* Vertical line */ + theta = M_PI / 2; + r = y; + } + /* Calculate cube root */ + theta /= 3; + r = 2 * cruckig_cbrt(r); + /* Convert to complex coordinate */ + const double ux = cos(theta) * r; + const double uyi = sin(theta) * r; + + cruckig_root_set_insert(&roots, ux - bover3a); + cruckig_root_set_insert(&roots, ux * cos120 - uyi * sin120 - bover3a); + cruckig_root_set_insert(&roots, ux * cos120 + uyi * sin120 - bover3a); + } else { + /* Sqrt is zero: two real solutions */ + const double www = -halfq; + const double w = 2 * cruckig_cbrt(www); + + cruckig_root_set_insert(&roots, w - bover3a); + cruckig_root_set_insert(&roots, w * cos120 - bover3a); + } + } + return roots; +} + +int cruckig_roots_solve_resolvent(double x[3], double a, double b, double c) { + const double cos120 = -0.50; + const double sin120 = 0.866025403784438646764; + + a /= 3; + const double a2 = a * a; + double q = a2 - b / 3; + const double r = (a * (2 * a2 - b) + c) / 2; + const double r2 = r * r; + const double q3 = q * q * q; + + if (r2 < q3) { + const double qsqrt = sqrt(q); + double t_val = r / (q * qsqrt); + if (t_val < -1.0) t_val = -1.0; + if (t_val > 1.0) t_val = 1.0; + q = -2 * qsqrt; + + const double theta = acos(t_val) / 3; + const double ux = cos(theta) * q; + const double uyi = sin(theta) * q; + x[0] = ux - a; + x[1] = ux * cos120 - uyi * sin120 - a; + x[2] = ux * cos120 + uyi * sin120 - a; + return 3; + } else { + double A = -cruckig_cbrt(fabs(r) + sqrt(r2 - q3)); + if (r < 0.0) { + A = -A; + } + const double B = (0.0 == A ? 0.0 : q / A); + + x[0] = (A + B) - a; + x[1] = -(A + B) / 2 - a; + x[2] = sqrt(3.0) * (A - B) / 2; + if (fabs(x[2]) < DBL_EPSILON) { + x[2] = x[1]; + return 2; + } + + return 1; + } +} + +CRUCKIG_HOT +CRuckigRootSet cruckig_roots_solve_quart_monic(double a, double b, double c, double d) { + CRuckigRootSet roots; + cruckig_root_set_init(&roots); + + if (fabs(d) < DBL_EPSILON) { + if (fabs(c) < DBL_EPSILON) { + cruckig_root_set_insert(&roots, 0.0); + + const double D = a * a - 4 * b; + if (fabs(D) < DBL_EPSILON) { + cruckig_root_set_insert(&roots, -a / 2); + } else if (D > 0.0) { + const double sqrtD = sqrt(D); + cruckig_root_set_insert(&roots, (-a - sqrtD) / 2); + cruckig_root_set_insert(&roots, (-a + sqrtD) / 2); + } + return roots; + } + + if (fabs(a) < DBL_EPSILON && fabs(b) < DBL_EPSILON) { + cruckig_root_set_insert(&roots, 0.0); + cruckig_root_set_insert(&roots, -cruckig_cbrt(c)); + return roots; + } + } + + const double a3 = -b; + const double b3 = a * c - 4 * d; + const double c3 = -a * a * d - c * c + 4 * b * d; + + double x3[3]; + const int number_zeroes = cruckig_roots_solve_resolvent(x3, a3, b3, c3); + + double y = x3[0]; + /* Choosing Y with maximal absolute value */ + if (number_zeroes != 1) { + if (fabs(x3[1]) > fabs(y)) { + y = x3[1]; + } + if (fabs(x3[2]) > fabs(y)) { + y = x3[2]; + } + } + + double q1, q2, p1, p2; + double D; + + D = y * y - 4 * d; + if (fabs(D) < DBL_EPSILON) { + q1 = q2 = y / 2; + D = a * a - 4 * (b - y); + if (fabs(D) < DBL_EPSILON) { + p1 = p2 = a / 2; + } else { + const double sqrtD = sqrt(D); + p1 = (a + sqrtD) / 2; + p2 = (a - sqrtD) / 2; + } + } else { + const double sqrtD = sqrt(D); + q1 = (y + sqrtD) / 2; + q2 = (y - sqrtD) / 2; + p1 = (a * q1 - c) / (q1 - q2); + p2 = (c - a * q2) / (q1 - q2); + } + + { + const double eps = 16 * DBL_EPSILON; + + D = p1 * p1 - 4 * q1; + if (fabs(D) < eps) { + cruckig_root_set_insert(&roots, -p1 / 2); + } else if (D > 0.0) { + const double sqrtD = sqrt(D); + cruckig_root_set_insert(&roots, (-p1 - sqrtD) / 2); + cruckig_root_set_insert(&roots, (-p1 + sqrtD) / 2); + } + + D = p2 * p2 - 4 * q2; + if (fabs(D) < eps) { + cruckig_root_set_insert(&roots, -p2 / 2); + } else if (D > 0.0) { + const double sqrtD = sqrt(D); + cruckig_root_set_insert(&roots, (-p2 - sqrtD) / 2); + cruckig_root_set_insert(&roots, (-p2 + sqrtD) / 2); + } + } + + return roots; +} + +double cruckig_roots_poly_eval(const double *p, size_t n, double x) { + if (n == 0) { + return 0.0; + } + + double retVal = 0.0; + + if (fabs(x) < DBL_EPSILON) { + retVal = p[n - 1]; + } else if (x == 1.0) { + for (int i = (int)n - 1; i >= 0; i--) { + retVal += p[i]; + } + } else { + double xn = 1.0; + for (int i = (int)n - 1; i >= 0; i--) { + retVal += p[i] * xn; + xn *= x; + } + } + + return retVal; +} + +void cruckig_roots_poly_derivative(const double *coeffs, size_t n, double *deriv) { + for (size_t i = 0; i < n - 1; ++i) { + deriv[i] = (double)(n - 1 - i) * coeffs[i]; + } +} + +double cruckig_roots_shrink_interval(const double *p, size_t n, double l, double h) { + const size_t maxIts = 128; + const double tolerance = 1e-14; + + const double fl = cruckig_roots_poly_eval(p, n, l); + const double fh = cruckig_roots_poly_eval(p, n, h); + if (fl == 0.0) { + return l; + } + if (fh == 0.0) { + return h; + } + if (fl > 0.0) { + /* swap l and h */ + double tmp = l; + l = h; + h = tmp; + } + + double rts = (l + h) / 2; + double dxold = fabs(h - l); + double dx = dxold; + + /* Compute derivative coefficients (n-1 elements) */ + double deriv[16]; /* max polynomial degree supported */ + cruckig_roots_poly_derivative(p, n, deriv); + size_t dn = n - 1; + + double f = cruckig_roots_poly_eval(p, n, rts); + double df = cruckig_roots_poly_eval(deriv, dn, rts); + double temp; + + for (size_t j = 0; j < maxIts; j++) { + if ((((rts - h) * df - f) * ((rts - l) * df - f) > 0.0) || (fabs(2 * f) > fabs(dxold * df))) { + dxold = dx; + dx = (h - l) / 2; + rts = l + dx; + if (l == rts) { + break; + } + } else { + dxold = dx; + dx = f / df; + temp = rts; + rts -= dx; + if (temp == rts) { + break; + } + } + + if (fabs(dx) < tolerance) { + break; + } + + f = cruckig_roots_poly_eval(p, n, rts); + df = cruckig_roots_poly_eval(deriv, dn, rts); + if (f < 0.0) { + l = rts; + } else { + h = rts; + } + } + + return rts; +} diff --git a/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/roots.h b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/roots.h new file mode 100644 index 0000000..b790c69 --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/roots.h @@ -0,0 +1,57 @@ +/* + * cruckig - Pure C99 port of the Ruckig trajectory generation library + * Copyright (c) 2025 Yang Yang + * Copyright (c) 2021 Lars Berscheid (original C++ Ruckig) + * + * License: MIT, see the LICENSE file in this directory. + */ +#ifndef CRUCKIG_ROOTS_H +#define CRUCKIG_ROOTS_H + +#include "cruckig_internal.h" + +/* A set of positive double roots, stored on the stack */ +typedef struct { + double data[4]; + size_t size; +} CRuckigRootSet; + +CRUCKIG_FORCE_INLINE void cruckig_root_set_init(CRuckigRootSet *s) { + s->size = 0; +} + +CRUCKIG_FORCE_INLINE void cruckig_root_set_insert(CRuckigRootSet *s, double value) { + if (value >= 0.0) { + s->data[s->size] = value; + s->size++; + } +} + +/* + * Cube root, portable replacement for cbrt() (not available in kernel). + * Optimized implementation from musl libc / FreeBSD libmsun. + */ +double cruckig_cbrt(double x); + +/* Sort the root set (simple insertion sort for small N) */ +void cruckig_root_set_sort(CRuckigRootSet *s); + +/* Solve a*x^3 + b*x^2 + c*x + d = 0, returning positive roots */ +CRuckigRootSet cruckig_roots_solve_cubic(double a, double b, double c, double d); + +/* Solve resolvent equation, returns number of zeros */ +int cruckig_roots_solve_resolvent(double x[3], double a, double b, double c); + +/* Solve monic quartic x^4 + a*x^3 + b*x^2 + c*x + d = 0 */ +CRuckigRootSet cruckig_roots_solve_quart_monic(double a, double b, double c, double d); + +/* Evaluate polynomial of order N at x. Coefficients in descending order: p[0]*x^(N-1) + ... + p[N-1] */ +double cruckig_roots_poly_eval(const double *p, size_t n, double x); + +/* Calculate derivative coefficients */ +void cruckig_roots_poly_derivative(const double *coeffs, size_t n, double *deriv); + +/* Safe Newton method: find root in [l, h] where p(l)*p(h) < 0 */ +double cruckig_roots_shrink_interval(const double *p, size_t n, double l, double h); + +#endif /* CRUCKIG_ROOTS_H */ diff --git a/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/trajectory.c b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/trajectory.c new file mode 100644 index 0000000..52c056a --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/trajectory.c @@ -0,0 +1,315 @@ +/* + * cruckig - Pure C99 port of the Ruckig trajectory generation library + * Copyright (c) 2025 Yang Yang + * Copyright (c) 2021 Lars Berscheid (original C++ Ruckig) + * + * License: MIT, see the LICENSE file in this directory. + */ +#include "trajectory.h" +#include "utils.h" + + +CRuckigTrajectory* cruckig_trajectory_create(size_t dofs) { + CRuckigTrajectory *traj = (CRuckigTrajectory*)cruckig_calloc(1, sizeof(CRuckigTrajectory)); + if (!traj) return NULL; + + traj->degrees_of_freedom = dofs; + traj->num_sections = 1; + traj->section_capacity = 1; + traj->duration = 0.0; + + traj->profiles = (CRuckigProfile*)cruckig_calloc(dofs, sizeof(CRuckigProfile)); + traj->cumulative_times = (double*)cruckig_calloc(1, sizeof(double)); + traj->independent_min_durations = (double*)cruckig_calloc(dofs, sizeof(double)); + traj->position_extrema = (CRuckigBound*)cruckig_calloc(dofs, sizeof(CRuckigBound)); + + if (!traj->profiles || !traj->cumulative_times || + !traj->independent_min_durations || !traj->position_extrema) { + cruckig_trajectory_destroy(traj); + return NULL; + } + + for (size_t dof = 0; dof < dofs; ++dof) { + cruckig_profile_init(&traj->profiles[dof]); + } + + return traj; +} + +void cruckig_trajectory_destroy(CRuckigTrajectory *traj) { + if (!traj) return; + cruckig_free(traj->profiles); + cruckig_free(traj->cumulative_times); + cruckig_free(traj->independent_min_durations); + cruckig_free(traj->position_extrema); + cruckig_free(traj); +} + +bool cruckig_trajectory_resize(CRuckigTrajectory *traj, size_t num_sections) { + if (!traj || num_sections == 0) return false; + + const size_t dofs = traj->degrees_of_freedom; + + if (num_sections > traj->section_capacity) { + CRuckigProfile *new_profiles = (CRuckigProfile*)cruckig_realloc( + traj->profiles, num_sections * dofs * sizeof(CRuckigProfile)); + double *new_times = (double*)cruckig_realloc( + traj->cumulative_times, num_sections * sizeof(double)); + + if (!new_profiles || !new_times) { + /* Restore on failure */ + if (new_profiles) traj->profiles = new_profiles; + if (new_times) traj->cumulative_times = new_times; + return false; + } + + traj->profiles = new_profiles; + traj->cumulative_times = new_times; + traj->section_capacity = num_sections; + + /* Initialize new profiles */ + for (size_t s = traj->num_sections; s < num_sections; ++s) { + for (size_t d = 0; d < dofs; ++d) { + cruckig_profile_init(&traj->profiles[s * dofs + d]); + } + traj->cumulative_times[s] = 0.0; + } + } + + traj->num_sections = num_sections; + return true; +} + +/* + * state_to_integrate_from: Determine the integration base state at a given time. + * Supports multi-section trajectories via binary search on cumulative_times. + */ +static void state_to_integrate_from(const CRuckigTrajectory *traj, double time, + size_t *new_section, + double *t_out, double *p_out, double *v_out, + double *a_out, double *j_out) +{ + const size_t dofs = traj->degrees_of_freedom; + const size_t nsec = traj->num_sections; + + if (time >= traj->duration) { + /* Past the end of trajectory */ + *new_section = nsec; + size_t last = nsec - 1; + for (size_t dof = 0; dof < dofs; ++dof) { + const CRuckigProfile *prof = &traj->profiles[last * dofs + dof]; + double t_pre = prof->brake.duration; + double t_diff = time - (traj->duration - (t_pre + prof->t_sum[6]) + t_pre + prof->t_sum[6]); + /* Simplify: time past the end of last section's profile */ + double section_start = (last > 0) ? traj->cumulative_times[last - 1] : 0.0; + t_diff = time - section_start - t_pre - prof->t_sum[6]; + t_out[dof] = t_diff; + p_out[dof] = prof->p[7]; + v_out[dof] = prof->v[7]; + a_out[dof] = prof->a[7]; + j_out[dof] = 0.0; + } + return; + } + + /* Binary search to find current section */ + size_t section = 0; + if (nsec > 1) { + size_t lo = 0, hi = nsec; + while (lo < hi) { + size_t mid = lo + (hi - lo) / 2; + if (traj->cumulative_times[mid] <= time) { + lo = mid + 1; + } else { + hi = mid; + } + } + section = lo; + if (section >= nsec) section = nsec - 1; + } + + *new_section = section; + + /* Time offset within this section */ + double section_start = (section > 0) ? traj->cumulative_times[section - 1] : 0.0; + double t_diff = time - section_start; + + for (size_t dof = 0; dof < dofs; ++dof) { + const CRuckigProfile *prof = &traj->profiles[section * dofs + dof]; + double t_diff_dof = t_diff; + + /* Brake pre-trajectory (only in first section, or in each section for waypoints) */ + if (prof->brake.duration > 0.0) { + if (t_diff_dof < prof->brake.duration) { + size_t index = (t_diff_dof < prof->brake.t[0]) ? 0 : 1; + if (index > 0) { + t_diff_dof -= prof->brake.t[index - 1]; + } + t_out[dof] = t_diff_dof; + p_out[dof] = prof->brake.p[index]; + v_out[dof] = prof->brake.v[index]; + a_out[dof] = prof->brake.a[index]; + j_out[dof] = prof->brake.j[index]; + continue; + } else { + t_diff_dof -= prof->brake.duration; + } + } + + /* Non-time synchronization: past the end of this DOF's profile */ + if (t_diff_dof >= prof->t_sum[6]) { + t_out[dof] = t_diff_dof - prof->t_sum[6]; + p_out[dof] = prof->p[7]; + v_out[dof] = prof->v[7]; + a_out[dof] = prof->a[7]; + j_out[dof] = 0.0; + continue; + } + + /* Binary search in t_sum[0..6] */ + size_t index_dof = 0; + { + size_t lo = 0, hi = 7; + while (lo < hi) { + size_t mid = lo + (hi - lo) / 2; + if (prof->t_sum[mid] <= t_diff_dof) { + lo = mid + 1; + } else { + hi = mid; + } + } + index_dof = lo; + } + + if (index_dof > 0) { + t_diff_dof -= prof->t_sum[index_dof - 1]; + } + + t_out[dof] = t_diff_dof; + p_out[dof] = prof->p[index_dof]; + v_out[dof] = prof->v[index_dof]; + a_out[dof] = prof->a[index_dof]; + j_out[dof] = prof->j[index_dof]; + } +} + +CRUCKIG_HOT +void cruckig_trajectory_at_time(const CRuckigTrajectory *traj, double time, + double * CRUCKIG_RESTRICT new_position, + double * CRUCKIG_RESTRICT new_velocity, + double * CRUCKIG_RESTRICT new_acceleration, + double * CRUCKIG_RESTRICT new_jerk, + size_t *new_section) +{ + const size_t dofs = traj->degrees_of_freedom; + + /* Implementation limit: max 16 DOF (stack-allocated work arrays) */ + double t_buf[16], p_buf[16], v_buf[16], a_buf[16], j_buf[16]; + const size_t ndofs = (dofs > 16) ? 16 : dofs; + + state_to_integrate_from(traj, time, new_section, t_buf, p_buf, v_buf, a_buf, j_buf); + + for (size_t dof = 0; dof < ndofs; ++dof) { + double p_out, v_out, a_out; + cruckig_integrate(t_buf[dof], p_buf[dof], v_buf[dof], a_buf[dof], j_buf[dof], + &p_out, &v_out, &a_out); + new_position[dof] = p_out; + new_velocity[dof] = v_out; + new_acceleration[dof] = a_out; + if (new_jerk) { + new_jerk[dof] = j_buf[dof]; + } + } +} + +void cruckig_trajectory_at_time_simple(const CRuckigTrajectory *traj, double time, + double *new_position, double *new_velocity, + double *new_acceleration) +{ + size_t new_section; + cruckig_trajectory_at_time(traj, time, new_position, new_velocity, + new_acceleration, NULL, &new_section); +} + +double cruckig_trajectory_get_duration(const CRuckigTrajectory *traj) { + return traj->duration; +} + +size_t cruckig_trajectory_get_intermediate_durations(const CRuckigTrajectory *traj, + double *out_durations) +{ + for (size_t s = 0; s < traj->num_sections; ++s) { + out_durations[s] = traj->cumulative_times[s]; + } + return traj->num_sections; +} + +void cruckig_trajectory_get_position_extrema(CRuckigTrajectory *traj) { + const size_t dofs = traj->degrees_of_freedom; + for (size_t dof = 0; dof < dofs; ++dof) { + /* Initialize from first section */ + CRuckigBound bound = cruckig_profile_get_position_extrema(&traj->profiles[dof]); + + /* Merge across all sections */ + for (size_t s = 1; s < traj->num_sections; ++s) { + double section_start = traj->cumulative_times[s - 1]; + CRuckigBound sb = cruckig_profile_get_position_extrema( + &traj->profiles[s * dofs + dof]); + if (sb.min < bound.min) { + bound.min = sb.min; + bound.t_min = sb.t_min + section_start; + } + if (sb.max > bound.max) { + bound.max = sb.max; + bound.t_max = sb.t_max + section_start; + } + } + + traj->position_extrema[dof] = bound; + } +} + +bool cruckig_trajectory_get_first_time_at_position(const CRuckigTrajectory *traj, + size_t dof, double position, + double *time, double time_after) +{ + if (dof >= traj->degrees_of_freedom) return false; + + const size_t dofs = traj->degrees_of_freedom; + + /* Search through all sections */ + for (size_t s = 0; s < traj->num_sections; ++s) { + double section_start = (s > 0) ? traj->cumulative_times[s - 1] : 0.0; + double adjusted_time_after = time_after - section_start; + if (adjusted_time_after < 0.0) adjusted_time_after = 0.0; + + if (cruckig_profile_get_first_state_at_position( + &traj->profiles[s * dofs + dof], position, time, adjusted_time_after)) { + *time += section_start; + return true; + } + } + + return false; +} + +void cruckig_trajectory_get_independent_min_durations(const CRuckigTrajectory *traj, + double *out_durations) +{ + for (size_t dof = 0; dof < traj->degrees_of_freedom; ++dof) { + out_durations[dof] = traj->independent_min_durations[dof]; + } +} + +const CRuckigProfile* cruckig_trajectory_get_profile(const CRuckigTrajectory *traj, size_t dof) +{ + if (dof >= traj->degrees_of_freedom) return NULL; + return &traj->profiles[dof]; +} + +const CRuckigProfile* cruckig_trajectory_get_section_profile(const CRuckigTrajectory *traj, + size_t section, size_t dof) +{ + if (section >= traj->num_sections || dof >= traj->degrees_of_freedom) return NULL; + return &traj->profiles[section * traj->degrees_of_freedom + dof]; +} diff --git a/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/trajectory.h b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/trajectory.h new file mode 100644 index 0000000..95600f0 --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/trajectory.h @@ -0,0 +1,71 @@ +/* + * cruckig - Pure C99 port of the Ruckig trajectory generation library + * Copyright (c) 2025 Yang Yang + * Copyright (c) 2021 Lars Berscheid (original C++ Ruckig) + * + * License: MIT, see the LICENSE file in this directory. + */ +#ifndef CRUCKIG_TRAJECTORY_H +#define CRUCKIG_TRAJECTORY_H + +#include "cruckig_internal.h" +#include "profile.h" + +typedef struct { + size_t degrees_of_freedom; + + /* Multi-section support: profiles[section * dofs + dof] */ + CRuckigProfile *profiles; /* Array of num_sections * dofs profiles */ + size_t num_sections; /* Number of sections (1 for state-to-state) */ + size_t section_capacity; /* Allocated capacity for sections */ + double duration; + double *cumulative_times; /* Array of num_sections cumulative durations */ + + double *independent_min_durations; /* Array of dofs */ + CRuckigBound *position_extrema; /* Array of dofs */ +} CRuckigTrajectory; + +/* Create trajectory for single-section (backward compatible) */ +CRuckigTrajectory* cruckig_trajectory_create(size_t dofs); +void cruckig_trajectory_destroy(CRuckigTrajectory *traj); + +/* Resize trajectory for multi-section (num_sections = max_waypoints + 1) */ +bool cruckig_trajectory_resize(CRuckigTrajectory *traj, size_t num_sections); + +/* Query trajectory state at time */ +void cruckig_trajectory_at_time(const CRuckigTrajectory *traj, double time, + double *new_position, double *new_velocity, + double *new_acceleration, double *new_jerk, + size_t *new_section); + +/* Simplified version without jerk/section */ +void cruckig_trajectory_at_time_simple(const CRuckigTrajectory *traj, double time, + double *new_position, double *new_velocity, + double *new_acceleration); + +double cruckig_trajectory_get_duration(const CRuckigTrajectory *traj); + +/* Get intermediate durations (cumulative times array). Returns num_sections. */ +size_t cruckig_trajectory_get_intermediate_durations(const CRuckigTrajectory *traj, + double *out_durations); + +/* Get position extrema for all DOFs */ +void cruckig_trajectory_get_position_extrema(CRuckigTrajectory *traj); + +/* Get first time at position for a DOF. Returns true if found. */ +bool cruckig_trajectory_get_first_time_at_position(const CRuckigTrajectory *traj, + size_t dof, double position, + double *time, double time_after); + +/* Get independent minimum durations (one per DOF). Caller provides array of dofs. */ +void cruckig_trajectory_get_independent_min_durations(const CRuckigTrajectory *traj, + double *out_durations); + +/* Get the underlying profile for a specific DOF in a section (read-only). */ +const CRuckigProfile* cruckig_trajectory_get_profile(const CRuckigTrajectory *traj, size_t dof); + +/* Get profile for specific section and DOF. */ +const CRuckigProfile* cruckig_trajectory_get_section_profile(const CRuckigTrajectory *traj, + size_t section, size_t dof); + +#endif /* CRUCKIG_TRAJECTORY_H */ diff --git a/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/utils.h b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/utils.h new file mode 100644 index 0000000..df18eb5 --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/utils.h @@ -0,0 +1,26 @@ +/* + * cruckig - Pure C99 port of the Ruckig trajectory generation library + * Copyright (c) 2025 Yang Yang + * Copyright (c) 2021 Lars Berscheid (original C++ Ruckig) + * + * License: MIT, see the LICENSE file in this directory. + */ +#ifndef CRUCKIG_UTILS_H +#define CRUCKIG_UTILS_H + +#include "cruckig_internal.h" + +CRUCKIG_FORCE_INLINE void cruckig_integrate(double t, double p0, double v0, double a0, double j, + double * CRUCKIG_RESTRICT p_out, + double * CRUCKIG_RESTRICT v_out, + double * CRUCKIG_RESTRICT a_out) { + *p_out = p0 + t * (v0 + t * (a0 / 2.0 + t * j / 6.0)); + *v_out = v0 + t * (a0 + t * j / 2.0); + *a_out = a0 + t * j; +} + +CRUCKIG_FORCE_INLINE double cruckig_pow2(double v) { + return v * v; +} + +#endif /* CRUCKIG_UTILS_H */ diff --git a/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/velocity.h b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/velocity.h new file mode 100644 index 0000000..5721120 --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/velocity.h @@ -0,0 +1,63 @@ +/* + * cruckig - Pure C99 port of the Ruckig trajectory generation library + * Copyright (c) 2025 Yang Yang + * Copyright (c) 2021 Lars Berscheid (original C++ Ruckig) + * + * License: MIT, see the LICENSE file in this directory. + */ +#ifndef CRUCKIG_VELOCITY_H +#define CRUCKIG_VELOCITY_H + +#include "cruckig_internal.h" +#include "profile.h" +#include "block.h" + +/* ---- Third Order Step 1 ---- */ +typedef struct { + double a0, af; + double _aMax, _aMin, _jMax; + double vd; + CRuckigProfile valid_profiles[3]; +} CRuckigVelocityThirdOrderStep1; + +void cruckig_vel3_step1_init(CRuckigVelocityThirdOrderStep1 *s, + double v0, double a0, double vf, double af, + double aMax, double aMin, double jMax); +bool cruckig_vel3_step1_get_profile(CRuckigVelocityThirdOrderStep1 *s, + const CRuckigProfile *input, CRuckigBlock *block); + +/* ---- Third Order Step 2 ---- */ +typedef struct { + double a0, tf, af; + double _aMax, _aMin, _jMax; + double vd, ad; +} CRuckigVelocityThirdOrderStep2; + +void cruckig_vel3_step2_init(CRuckigVelocityThirdOrderStep2 *s, + double tf, double v0, double a0, double vf, double af, + double aMax, double aMin, double jMax); +bool cruckig_vel3_step2_get_profile(CRuckigVelocityThirdOrderStep2 *s, CRuckigProfile *profile); + +/* ---- Second Order Step 1 ---- */ +typedef struct { + double _aMax, _aMin; + double vd; +} CRuckigVelocitySecondOrderStep1; + +void cruckig_vel2_step1_init(CRuckigVelocitySecondOrderStep1 *s, + double v0, double vf, double aMax, double aMin); +bool cruckig_vel2_step1_get_profile(CRuckigVelocitySecondOrderStep1 *s, + const CRuckigProfile *input, CRuckigBlock *block); + +/* ---- Second Order Step 2 ---- */ +typedef struct { + double tf; + double _aMax, _aMin; + double vd; +} CRuckigVelocitySecondOrderStep2; + +void cruckig_vel2_step2_init(CRuckigVelocitySecondOrderStep2 *s, + double tf, double v0, double vf, double aMax, double aMin); +bool cruckig_vel2_step2_get_profile(CRuckigVelocitySecondOrderStep2 *s, CRuckigProfile *profile); + +#endif /* CRUCKIG_VELOCITY_H */ diff --git a/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/velocity_second_step1.c b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/velocity_second_step1.c new file mode 100644 index 0000000..38d8239 --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/velocity_second_step1.c @@ -0,0 +1,40 @@ +/* + * cruckig - Pure C99 port of the Ruckig trajectory generation library + * Copyright (c) 2025 Yang Yang + * Copyright (c) 2021 Lars Berscheid (original C++ Ruckig) + * + * License: MIT, see the LICENSE file in this directory. + */ +#include "velocity.h" +#include "block.h" +#include "profile.h" + +void cruckig_vel2_step1_init(CRuckigVelocitySecondOrderStep1 *s, + double v0, double vf, double aMax, double aMin) +{ + s->_aMax = aMax; + s->_aMin = aMin; + s->vd = vf - v0; +} + +bool cruckig_vel2_step1_get_profile(CRuckigVelocitySecondOrderStep1 *s, + const CRuckigProfile *input, CRuckigBlock *block) +{ + CRuckigProfile *p = &block->p_min; + cruckig_profile_set_boundary_from_profile(p, input); + + const double af = (s->vd > 0) ? s->_aMax : s->_aMin; + p->t[0] = 0; + p->t[1] = s->vd / af; + p->t[2] = 0; + p->t[3] = 0; + p->t[4] = 0; + p->t[5] = 0; + p->t[6] = 0; + + if (cruckig_profile_check_for_second_order_velocity(p, ControlSignsUDDU, ReachedLimitsACC0, af)) { + block->t_min = p->t_sum[6] + p->brake.duration + p->accel.duration; + return true; + } + return false; +} diff --git a/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/velocity_second_step2.c b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/velocity_second_step2.c new file mode 100644 index 0000000..349d3d2 --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/velocity_second_step2.c @@ -0,0 +1,39 @@ +/* + * cruckig - Pure C99 port of the Ruckig trajectory generation library + * Copyright (c) 2025 Yang Yang + * Copyright (c) 2021 Lars Berscheid (original C++ Ruckig) + * + * License: MIT, see the LICENSE file in this directory. + */ +#include "velocity.h" +#include "block.h" +#include "profile.h" + +void cruckig_vel2_step2_init(CRuckigVelocitySecondOrderStep2 *s, + double tf, double v0, double vf, double aMax, double aMin) +{ + s->tf = tf; + s->_aMax = aMax; + s->_aMin = aMin; + s->vd = vf - v0; +} + +bool cruckig_vel2_step2_get_profile(CRuckigVelocitySecondOrderStep2 *s, CRuckigProfile *profile) +{ + const double af = s->vd / s->tf; + + profile->t[0] = 0; + profile->t[1] = s->tf; + profile->t[2] = 0; + profile->t[3] = 0; + profile->t[4] = 0; + profile->t[5] = 0; + profile->t[6] = 0; + + if (cruckig_profile_check_for_second_order_velocity_with_timing_full(profile, ControlSignsUDDU, ReachedLimitsNONE, s->tf, af, s->_aMax, s->_aMin)) { + profile->pf = profile->p[7]; + return true; + } + + return false; +} diff --git a/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/velocity_third_step1.c b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/velocity_third_step1.c new file mode 100644 index 0000000..32499e4 --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/velocity_third_step1.c @@ -0,0 +1,187 @@ +/* + * cruckig - Pure C99 port of the Ruckig trajectory generation library + * Copyright (c) 2025 Yang Yang + * Copyright (c) 2021 Lars Berscheid (original C++ Ruckig) + * + * License: MIT, see the LICENSE file in this directory. + */ + +#include "velocity.h" +#include "block.h" +#include "profile.h" + +/* ---- Internal helper functions ---- */ + +static void time_acc0(const CRuckigVelocityThirdOrderStep1 *s, + CRuckigProfile *valid_profiles, size_t *counter, + double aMax, double aMin, double jMax, bool return_after_found) +{ + (void)return_after_found; + + CRuckigProfile *profile = &valid_profiles[*counter]; + + profile->t[0] = (-s->a0 + aMax) / jMax; + profile->t[1] = (s->a0 * s->a0 + s->af * s->af) / (2 * aMax * jMax) - aMax / jMax + s->vd / aMax; + profile->t[2] = (-s->af + aMax) / jMax; + profile->t[3] = 0; + profile->t[4] = 0; + profile->t[5] = 0; + profile->t[6] = 0; + + if (cruckig_profile_check_for_velocity(profile, ControlSignsUDDU, ReachedLimitsACC0, jMax, aMax, aMin)) { + (*counter)++; + if (*counter < 3) { + cruckig_profile_set_boundary_from_profile(&valid_profiles[*counter], profile); + } + } +} + +static void time_none(const CRuckigVelocityThirdOrderStep1 *s, + CRuckigProfile *valid_profiles, size_t *counter, + double aMax, double aMin, double jMax, bool return_after_found) +{ + double h1 = (s->a0 * s->a0 + s->af * s->af) / 2 + jMax * s->vd; + if (h1 >= 0.0) { + h1 = sqrt(h1); + + /* Solution 1 */ + { + CRuckigProfile *profile = &valid_profiles[*counter]; + + profile->t[0] = -(s->a0 + h1) / jMax; + profile->t[1] = 0; + profile->t[2] = -(s->af + h1) / jMax; + profile->t[3] = 0; + profile->t[4] = 0; + profile->t[5] = 0; + profile->t[6] = 0; + + if (cruckig_profile_check_for_velocity(profile, ControlSignsUDDU, ReachedLimitsNONE, jMax, aMax, aMin)) { + (*counter)++; + if (*counter < 3) { + cruckig_profile_set_boundary_from_profile(&valid_profiles[*counter], profile); + } + if (return_after_found) { + return; + } + } + } + + /* Solution 2 */ + { + CRuckigProfile *profile = &valid_profiles[*counter]; + + profile->t[0] = (-s->a0 + h1) / jMax; + profile->t[1] = 0; + profile->t[2] = (-s->af + h1) / jMax; + profile->t[3] = 0; + profile->t[4] = 0; + profile->t[5] = 0; + profile->t[6] = 0; + + if (cruckig_profile_check_for_velocity(profile, ControlSignsUDDU, ReachedLimitsNONE, jMax, aMax, aMin)) { + (*counter)++; + if (*counter < 3) { + cruckig_profile_set_boundary_from_profile(&valid_profiles[*counter], profile); + } + } + } + } +} + +static bool time_all_single_step(const CRuckigVelocityThirdOrderStep1 *s, + CRuckigProfile *profile, + double aMax, double aMin, double jMax) +{ + (void)jMax; + + if (fabs(s->af - s->a0) > DBL_EPSILON) { + return false; + } + + profile->t[0] = 0; + profile->t[1] = 0; + profile->t[2] = 0; + profile->t[3] = 0; + profile->t[4] = 0; + profile->t[5] = 0; + profile->t[6] = 0; + + if (fabs(s->a0) > DBL_EPSILON) { + profile->t[3] = s->vd / s->a0; + if (cruckig_profile_check_for_velocity(profile, ControlSignsUDDU, ReachedLimitsNONE, 0.0, aMax, aMin)) { + return true; + } + + } else if (fabs(s->vd) < DBL_EPSILON) { + if (cruckig_profile_check_for_velocity(profile, ControlSignsUDDU, ReachedLimitsNONE, 0.0, aMax, aMin)) { + return true; + } + } + + return false; +} + + +/* ---- Public interface ---- */ + +void cruckig_vel3_step1_init(CRuckigVelocityThirdOrderStep1 *s, + double v0, double a0, double vf, double af, + double aMax, double aMin, double jMax) +{ + s->a0 = a0; + s->af = af; + s->_aMax = aMax; + s->_aMin = aMin; + s->_jMax = jMax; + s->vd = vf - v0; +} + +bool cruckig_vel3_step1_get_profile(CRuckigVelocityThirdOrderStep1 *s, + const CRuckigProfile *input, CRuckigBlock *block) +{ + /* Zero-limits special case */ + if (s->_jMax == 0.0) { + CRuckigProfile *p = &block->p_min; + cruckig_profile_set_boundary_from_profile(p, input); + + if (time_all_single_step(s, p, s->_aMax, s->_aMin, s->_jMax)) { + block->t_min = p->t_sum[6] + p->brake.duration + p->accel.duration; + if (fabs(s->a0) > DBL_EPSILON) { + block->a.valid = true; + block->a.left = block->t_min; + block->a.right = INFINITY; + } + return true; + } + return false; + } + + size_t valid_profile_counter = 0; + cruckig_profile_set_boundary_from_profile(&s->valid_profiles[0], input); + + if (fabs(s->af) < DBL_EPSILON) { + /* There is no blocked interval when af==0, so return after first found profile */ + const double aMax = (s->vd >= 0) ? s->_aMax : s->_aMin; + const double aMin = (s->vd >= 0) ? s->_aMin : s->_aMax; + const double jMax = (s->vd >= 0) ? s->_jMax : -s->_jMax; + + time_none(s, s->valid_profiles, &valid_profile_counter, aMax, aMin, jMax, true); + if (valid_profile_counter > 0) { goto return_block; } + time_acc0(s, s->valid_profiles, &valid_profile_counter, aMax, aMin, jMax, true); + if (valid_profile_counter > 0) { goto return_block; } + + time_none(s, s->valid_profiles, &valid_profile_counter, aMin, aMax, -jMax, true); + if (valid_profile_counter > 0) { goto return_block; } + time_acc0(s, s->valid_profiles, &valid_profile_counter, aMin, aMax, -jMax, true); + + } else { + time_none(s, s->valid_profiles, &valid_profile_counter, s->_aMax, s->_aMin, s->_jMax, false); + time_none(s, s->valid_profiles, &valid_profile_counter, s->_aMin, s->_aMax, -s->_jMax, false); + time_acc0(s, s->valid_profiles, &valid_profile_counter, s->_aMax, s->_aMin, s->_jMax, false); + time_acc0(s, s->valid_profiles, &valid_profile_counter, s->_aMin, s->_aMax, -s->_jMax, false); + } + +return_block: + return cruckig_block_calculate(block, s->valid_profiles, valid_profile_counter, 3); +} diff --git a/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/velocity_third_step2.c b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/velocity_third_step2.c new file mode 100644 index 0000000..3b9f8b1 --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/tp/cruckig/velocity_third_step2.c @@ -0,0 +1,146 @@ +/* + * cruckig - Pure C99 port of the Ruckig trajectory generation library + * Copyright (c) 2025 Yang Yang + * Copyright (c) 2021 Lars Berscheid (original C++ Ruckig) + * + * License: MIT, see the LICENSE file in this directory. + */ + +#include "velocity.h" +#include "block.h" +#include "profile.h" + +/* ---- Internal helper functions ---- */ + +static bool time_acc0(CRuckigVelocityThirdOrderStep2 *s, CRuckigProfile *profile, + double aMax, double aMin, double jMax) +{ + /* UD Solution 1/2 */ + { + const double h1 = sqrt((-s->ad * s->ad + 2 * jMax * ((s->a0 + s->af) * s->tf - 2 * s->vd)) / (jMax * jMax) + s->tf * s->tf); + + profile->t[0] = s->ad / (2 * jMax) + (s->tf - h1) / 2; + profile->t[1] = h1; + profile->t[2] = s->tf - (profile->t[0] + h1); + profile->t[3] = 0; + profile->t[4] = 0; + profile->t[5] = 0; + profile->t[6] = 0; + + if (cruckig_profile_check_for_velocity_with_timing(profile, ControlSignsUDDU, ReachedLimitsACC0, s->tf, jMax, aMax, aMin)) { + profile->pf = profile->p[7]; + return true; + } + } + + /* UU Solution */ + { + const double h1 = (-s->ad + jMax * s->tf); + + profile->t[0] = -s->ad * s->ad / (2 * jMax * h1) + (s->vd - s->a0 * s->tf) / h1; + profile->t[1] = -s->ad / jMax + s->tf; + profile->t[2] = 0; + profile->t[3] = 0; + profile->t[4] = 0; + profile->t[5] = 0; + profile->t[6] = s->tf - (profile->t[0] + profile->t[1]); + + if (cruckig_profile_check_for_velocity_with_timing(profile, ControlSignsUDDU, ReachedLimitsACC0, s->tf, jMax, aMax, aMin)) { + profile->pf = profile->p[7]; + return true; + } + } + + /* UU Solution - 2 step */ + { + profile->t[0] = 0; + profile->t[1] = -s->ad / jMax + s->tf; + profile->t[2] = 0; + profile->t[3] = 0; + profile->t[4] = 0; + profile->t[5] = 0; + profile->t[6] = s->ad / jMax; + + if (cruckig_profile_check_for_velocity_with_timing(profile, ControlSignsUDDU, ReachedLimitsACC0, s->tf, jMax, aMax, aMin)) { + profile->pf = profile->p[7]; + return true; + } + } + + return false; +} + +static bool time_none(CRuckigVelocityThirdOrderStep2 *s, CRuckigProfile *profile, + double aMax, double aMin, double jMax) +{ + if (fabs(s->a0) < DBL_EPSILON && fabs(s->af) < DBL_EPSILON && fabs(s->vd) < DBL_EPSILON) { + profile->t[0] = 0; + profile->t[1] = s->tf; + profile->t[2] = 0; + profile->t[3] = 0; + profile->t[4] = 0; + profile->t[5] = 0; + profile->t[6] = 0; + + if (cruckig_profile_check_for_velocity_with_timing(profile, ControlSignsUDDU, ReachedLimitsNONE, s->tf, jMax, aMax, aMin)) { + profile->pf = profile->p[7]; + return true; + } + } + + /* UD Solution 1/2 */ + { + const double h1 = 2 * (s->af * s->tf - s->vd); + + profile->t[0] = h1 / s->ad; + profile->t[1] = s->tf - profile->t[0]; + profile->t[2] = 0; + profile->t[3] = 0; + profile->t[4] = 0; + profile->t[5] = 0; + profile->t[6] = 0; + + const double jf = s->ad * s->ad / h1; + + if (fabs(jf) < fabs(jMax) + 1e-12 && cruckig_profile_check_for_velocity_with_timing(profile, ControlSignsUDDU, ReachedLimitsNONE, s->tf, jf, aMax, aMin)) { + profile->pf = profile->p[7]; + return true; + } + } + + return false; +} + +static bool check_all(CRuckigVelocityThirdOrderStep2 *s, CRuckigProfile *profile, + double aMax, double aMin, double jMax) +{ + return time_acc0(s, profile, aMax, aMin, jMax) || time_none(s, profile, aMax, aMin, jMax); +} + + +/* ---- Public interface ---- */ + +void cruckig_vel3_step2_init(CRuckigVelocityThirdOrderStep2 *s, + double tf, double v0, double a0, double vf, double af, + double aMax, double aMin, double jMax) +{ + s->a0 = a0; + s->tf = tf; + s->af = af; + s->_aMax = aMax; + s->_aMin = aMin; + s->_jMax = jMax; + s->vd = vf - v0; + s->ad = af - a0; +} + +bool cruckig_vel3_step2_get_profile(CRuckigVelocityThirdOrderStep2 *s, CRuckigProfile *profile) +{ + /* Test all cases to get ones that match */ + /* However we should guess which one is correct and try them first... */ + if (s->vd > 0) { + return check_all(s, profile, s->_aMax, s->_aMin, s->_jMax) || check_all(s, profile, s->_aMin, s->_aMax, -s->_jMax); + } + + return check_all(s, profile, s->_aMin, s->_aMax, -s->_jMax) || check_all(s, profile, s->_aMax, s->_aMin, s->_jMax); +} diff --git a/wasm-port/vendor/linuxcnc/src/emc/tp/ruckig_wrapper.c b/wasm-port/vendor/linuxcnc/src/emc/tp/ruckig_wrapper.c new file mode 100644 index 0000000..d2c0ba1 --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/tp/ruckig_wrapper.c @@ -0,0 +1,680 @@ +/******************************************************************** +* Description: ruckig_wrapper.c +* Cruckig (pure C) trajectory planning library wrapper implementation +* +* This file provides a C wrapper around the Cruckig C library +* for S-curve trajectory planning in LinuxCNC. +* Replaces the C++ Ruckig implementation to enable RTAI kernel builds. +* +* License: GPL Version 2 +* System: Linux +* Original Author: 杨阳 (mika-net@outlook.com) +* Cruckig port: LinuxCNC contributors +* +* Copyright (c) 2024-2026 All rights reserved. +********************************************************************/ + +#include "ruckig_wrapper.h" +#include +#include +#include + +/* LinuxCNC precision constants (consistent with tp_types.h) */ +#ifndef TP_POS_EPSILON +#define TP_POS_EPSILON 1e-12 +#endif +#ifndef TP_VEL_EPSILON +#define TP_VEL_EPSILON 1e-8 +#endif + +/* Cruckig C headers */ +#include "cruckig/cruckig.h" + +/* Internal implementation struct */ +struct RuckigPlannerImpl { + CRuckig *otg; /* cruckig planner instance */ + CRuckigInputParameter *input; /* input parameters */ + CRuckigTrajectory *trajectory; /* trajectory result */ + double cycle_time; /* cycle time */ + int planned; /* whether planning has been done */ + double start_time; /* trajectory start time */ + double target_pos; /* target position (used for precision correction) */ + double target_vel; /* target velocity (used for precision correction) */ + double target_acc; /* target acceleration (used for precision correction) */ + int use_position_control; /* 1=position control, 0=velocity control */ + double last_actual_acc; /* previous actual acceleration (for jerk calculation) */ + int is_first_cycle; /* first cycle after replanning */ + int enable_logging; /* 1=enabled, 0=disabled */ +}; + +/* Helper macro: conditionally output log based on planner's logging setting */ +#define RUCKIG_LOG_IF_ENABLED(planner, level, fmt, ...) \ + do { \ + if (planner) { \ + struct RuckigPlannerImpl *_impl = (struct RuckigPlannerImpl *)planner; \ + if (_impl->enable_logging) { \ + rtapi_print_msg(level, fmt, ##__VA_ARGS__); \ + } \ + } else { \ + rtapi_print_msg(level, fmt, ##__VA_ARGS__); \ + } \ + } while (0) + +RuckigPlanner ruckig_create(double cycle_time) { + if (cycle_time <= 0.0) { + rtapi_print_msg(RTAPI_MSG_ERR, "ruckig_create: invalid cycle_time %f\n", cycle_time); + return NULL; + } + + struct RuckigPlannerImpl *impl = (struct RuckigPlannerImpl *)rtapi_kmalloc(sizeof(struct RuckigPlannerImpl), RTAPI_GFP_KERNEL); + if (!impl) { + rtapi_print_msg(RTAPI_MSG_ERR, "ruckig_create: memory allocation failed\n"); + return NULL; + } + + impl->otg = cruckig_create(1, cycle_time); + impl->input = cruckig_input_create(1); + impl->trajectory = cruckig_trajectory_create(1); + + if (!impl->otg || !impl->input || !impl->trajectory) { + rtapi_print_msg(RTAPI_MSG_ERR, "ruckig_create: cruckig allocation failed\n"); + if (impl->otg) cruckig_destroy(impl->otg); + if (impl->input) cruckig_input_destroy(impl->input); + if (impl->trajectory) cruckig_trajectory_destroy(impl->trajectory); + rtapi_kfree(impl); + return NULL; + } + + impl->cycle_time = cycle_time; + impl->planned = 0; + impl->start_time = 0.0; + impl->target_pos = 0.0; + impl->target_vel = 0.0; + impl->target_acc = 0.0; + impl->use_position_control = 0; + impl->last_actual_acc = 0.0; + impl->is_first_cycle = 0; + impl->enable_logging = 1; + + return (RuckigPlanner)impl; +} + +void ruckig_destroy(RuckigPlanner planner) { + if (planner) { + struct RuckigPlannerImpl *impl = (struct RuckigPlannerImpl *)planner; + if (impl->otg) cruckig_destroy(impl->otg); + if (impl->input) cruckig_input_destroy(impl->input); + if (impl->trajectory) cruckig_trajectory_destroy(impl->trajectory); + rtapi_kfree(impl); + } +} + +/* Helper: copy trajectory state for backup/restore on planning failure. + * We cannot just memcpy the CRuckigTrajectory because it contains owned pointers. + * Instead we save/restore the profile data and scalar fields. */ +struct TrajectoryBackup { + CRuckigProfile profile; /* single-DOF single-section profile copy */ + double duration; + double cumulative_time; + double independent_min_duration; + CRuckigBound position_extremum; +}; + +static void backup_trajectory(const CRuckigTrajectory *traj, struct TrajectoryBackup *bk) { + bk->duration = traj->duration; + if (traj->profiles) + bk->profile = traj->profiles[0]; /* 1 DOF, 1 section */ + if (traj->cumulative_times) + bk->cumulative_time = traj->cumulative_times[0]; + if (traj->independent_min_durations) + bk->independent_min_duration = traj->independent_min_durations[0]; + if (traj->position_extrema) + bk->position_extremum = traj->position_extrema[0]; +} + +static void restore_trajectory(CRuckigTrajectory *traj, const struct TrajectoryBackup *bk) { + traj->duration = bk->duration; + if (traj->profiles) + traj->profiles[0] = bk->profile; + if (traj->cumulative_times) + traj->cumulative_times[0] = bk->cumulative_time; + if (traj->independent_min_durations) + traj->independent_min_durations[0] = bk->independent_min_duration; + if (traj->position_extrema) + traj->position_extrema[0] = bk->position_extremum; +} + +/* Helper: handle cruckig result codes, return 0 on success, -1 or -2 on failure. + * On failure with a previous plan, restores the backup. */ +static int handle_result(CRuckigResult result, RuckigPlanner planner, + const char *func_name, + int had_previous_plan, + const struct TrajectoryBackup *bk, + double bk_target_pos, double bk_target_vel, + int bk_use_position_control, double bk_last_actual_acc) { + struct RuckigPlannerImpl *impl = (struct RuckigPlannerImpl *)planner; + + if (result == CRuckigWorking || result == CRuckigFinished) { + if (result == CRuckigFinished) { + double duration = cruckig_trajectory_get_duration(impl->trajectory); + if (duration < 0.001) { + RUCKIG_LOG_IF_ENABLED(planner, RTAPI_MSG_INFO, + "%s: already at target (duration=%f)\n", func_name, duration); + } else { + RUCKIG_LOG_IF_ENABLED(planner, RTAPI_MSG_INFO, + "%s: trajectory finished (duration=%f)\n", func_name, duration); + } + } + return 0; /* success */ + } + + /* Planning failed: restore previous trajectory if it exists */ + if (had_previous_plan) { + restore_trajectory(impl->trajectory, bk); + impl->target_pos = bk_target_pos; + impl->target_vel = bk_target_vel; + impl->use_position_control = bk_use_position_control; + impl->last_actual_acc = bk_last_actual_acc; + RUCKIG_LOG_IF_ENABLED(planner, RTAPI_MSG_INFO, + "%s: planning failed, restored previous trajectory\n", func_name); + } + + /* Log error */ + switch (result) { + case CRuckigErrorInvalidInput: + RUCKIG_LOG_IF_ENABLED(planner, RTAPI_MSG_ERR, + "%s: invalid input parameters\n", func_name); + break; + case CRuckigErrorTrajectoryDuration: + RUCKIG_LOG_IF_ENABLED(planner, RTAPI_MSG_ERR, + "%s: trajectory duration exceeds numerical limits\n", func_name); + break; + case CRuckigErrorPositionalLimits: + RUCKIG_LOG_IF_ENABLED(planner, RTAPI_MSG_ERR, + "%s: positional limits exceeded\n", func_name); + break; + case CRuckigErrorZeroLimits: + RUCKIG_LOG_IF_ENABLED(planner, RTAPI_MSG_ERR, + "%s: zero limits conflict\n", func_name); + break; + case CRuckigErrorExecutionTimeCalculation: + return -2; + case CRuckigErrorSynchronizationCalculation: + RUCKIG_LOG_IF_ENABLED(planner, RTAPI_MSG_ERR, + "%s: synchronization calculation error\n", func_name); + break; + case CRuckigError: + RUCKIG_LOG_IF_ENABLED(planner, RTAPI_MSG_ERR, + "%s: general error\n", func_name); + break; + default: + RUCKIG_LOG_IF_ENABLED(planner, RTAPI_MSG_ERR, + "%s: unknown error result %d\n", func_name, (int)result); + break; + } + return -1; +} + +int ruckig_plan_position(RuckigPlanner planner, + double current_pos, + double current_vel, + double current_acc, + double target_pos, + double target_vel, + double target_acc, + double min_vel, + double max_vel, + double max_acc, + double max_jerk) { + if (!planner) { + return -1; + } + + struct RuckigPlannerImpl *impl = (struct RuckigPlannerImpl *)planner; + + /* Parameter validation */ + if (max_vel <= 0.0 || max_acc <= 0.0 || max_jerk <= 0.0) { + RUCKIG_LOG_IF_ENABLED(planner, RTAPI_MSG_ERR, + "ruckig_plan_position: invalid limits (v=%f, a=%f, j=%f)\n", + max_vel, max_acc, max_jerk); + return -1; + } + + /* Set input parameters (position control mode) */ + impl->input->control_interface = CRuckigPosition; + impl->input->synchronization = CRuckigSyncTime; + + impl->input->current_position[0] = current_pos; + impl->input->current_velocity[0] = current_vel; + impl->input->current_acceleration[0] = current_acc; + impl->input->target_position[0] = target_pos; + impl->input->target_velocity[0] = target_vel; + impl->input->target_acceleration[0] = target_acc; + impl->input->max_velocity[0] = max_vel; + impl->input->max_acceleration[0] = max_acc; + impl->input->max_jerk[0] = max_jerk; + + /* Set min_velocity: cruckig uses NULL for default (-max), or a pointer for explicit */ + if (impl->input->min_velocity == NULL) { + impl->input->min_velocity = (double *)rtapi_kmalloc(sizeof(double), RTAPI_GFP_KERNEL); + if (!impl->input->min_velocity) return -1; + } + impl->input->min_velocity[0] = min_vel; + + /* Backup trajectory on failure */ + int had_previous_plan = impl->planned; + struct TrajectoryBackup bk; + double bk_target_pos = 0.0, bk_target_vel = 0.0, bk_last_actual_acc = 0.0; + int bk_use_position_control = 0; + + if (had_previous_plan) { + backup_trajectory(impl->trajectory, &bk); + bk_target_pos = impl->target_pos; + bk_target_vel = impl->target_vel; + bk_use_position_control = impl->use_position_control; + bk_last_actual_acc = impl->last_actual_acc; + } + + /* Execute planning */ + CRuckigResult result = cruckig_calculate(impl->otg, impl->input, impl->trajectory); + + int rc = handle_result(result, planner, "ruckig_plan_position", + had_previous_plan, &bk, + bk_target_pos, bk_target_vel, + bk_use_position_control, bk_last_actual_acc); + if (rc != 0) return rc; + + /* Update state on success */ + int was_planned = impl->planned; + if (!was_planned) { + impl->last_actual_acc = current_acc; + } + + impl->planned = 1; + impl->start_time = 0.0; + impl->target_pos = target_pos; + impl->target_vel = target_vel; + impl->target_acc = target_acc; + impl->use_position_control = 1; + impl->is_first_cycle = 1; + return 0; +} + +int ruckig_plan_velocity(RuckigPlanner planner, + double current_vel, + double current_acc, + double target_vel, + double target_acc, + double min_vel, + double max_acc, + double max_jerk) { + if (!planner) { + return -1; + } + + struct RuckigPlannerImpl *impl = (struct RuckigPlannerImpl *)planner; + + /* Parameter validation */ + if (max_acc <= 0.0 || max_jerk <= 0.0) { + RUCKIG_LOG_IF_ENABLED(planner, RTAPI_MSG_ERR, + "ruckig_plan_velocity: invalid limits (a=%f, j=%f)\n", + max_acc, max_jerk); + return -1; + } + + /* Set input parameters (velocity control mode) */ + impl->input->control_interface = CRuckigVelocity; + impl->input->synchronization = CRuckigSyncNone; + + impl->input->current_position[0] = 0.0; + impl->input->current_velocity[0] = current_vel; + impl->input->current_acceleration[0] = current_acc; + impl->input->target_position[0] = 0.0; + impl->input->target_velocity[0] = target_vel; + impl->input->target_acceleration[0] = target_acc; + impl->input->max_velocity[0] = INFINITY; + impl->input->max_acceleration[0] = max_acc; + impl->input->max_jerk[0] = max_jerk; + + /* Set min_velocity */ + if (impl->input->min_velocity == NULL) { + impl->input->min_velocity = (double *)rtapi_kmalloc(sizeof(double), RTAPI_GFP_KERNEL); + if (!impl->input->min_velocity) return -1; + } + impl->input->min_velocity[0] = min_vel; + + /* Backup trajectory on failure */ + int had_previous_plan = impl->planned; + struct TrajectoryBackup bk; + double bk_target_pos = 0.0, bk_target_vel = 0.0, bk_last_actual_acc = 0.0; + int bk_use_position_control = 0; + + if (had_previous_plan) { + backup_trajectory(impl->trajectory, &bk); + bk_target_pos = impl->target_pos; + bk_target_vel = impl->target_vel; + bk_use_position_control = impl->use_position_control; + bk_last_actual_acc = impl->last_actual_acc; + } + + /* Execute planning */ + CRuckigResult result = cruckig_calculate(impl->otg, impl->input, impl->trajectory); + + int rc = handle_result(result, planner, "ruckig_plan_velocity", + had_previous_plan, &bk, + bk_target_pos, bk_target_vel, + bk_use_position_control, bk_last_actual_acc); + if (rc != 0) return rc; + + /* Update state on success */ + int was_planned = impl->planned; + if (!was_planned) { + impl->last_actual_acc = current_acc; + } + + impl->planned = 1; + impl->start_time = 0.0; + impl->target_pos = 0.0; + impl->target_vel = target_vel; + impl->target_acc = target_acc; + impl->use_position_control = 0; + impl->is_first_cycle = 1; + return 0; +} + +int ruckig_at_time(RuckigPlanner planner, + double time, + double *pos, + double *vel, + double *acc, + double *jerk) { + if (!planner || !pos || !vel || !acc || !jerk) { + return -1; + } + + struct RuckigPlannerImpl *impl = (struct RuckigPlannerImpl *)planner; + + if (!impl->planned) { + rtapi_print_msg(RTAPI_MSG_ERR, "ruckig_at_time: trajectory not planned\n"); + return -1; + } + + double duration = cruckig_trajectory_get_duration(impl->trajectory); + + /* Clamp time */ + double query_time = time; + if (time < 0.0) { + rtapi_print_msg(RTAPI_MSG_ERR, "ruckig_at_time: time %f is negative\n", time); + return -1; + } + if (time > duration) { + query_time = duration; + } + + /* Get state at specified time */ + double new_pos, new_vel, new_acc, new_jerk_unused; + size_t new_section; + cruckig_trajectory_at_time(impl->trajectory, query_time, + &new_pos, &new_vel, &new_acc, &new_jerk_unused, + &new_section); + + *pos = new_pos; + *vel = new_vel; + *acc = new_acc; + + /* Precision correction: ensure position and velocity exactly match target values */ + if (impl->use_position_control) { + const double TIME_THRESHOLD = fmax(duration * 0.1, impl->cycle_time * 10.0); + const double POS_ERROR_THRESHOLD = 1e-6; + + if (time >= duration - TIME_THRESHOLD || time >= duration) { + double pos_error = fabs(*pos - impl->target_pos); + if (pos_error < POS_ERROR_THRESHOLD) { + *pos = impl->target_pos; + } + + if (time >= duration) { + *vel = impl->target_vel; + } + /* During trajectory: let S-curve complete naturally */ + } + } else { + /* Velocity control mode: only correct at trajectory end */ + if (time >= duration) { + *vel = impl->target_vel; + *acc = impl->target_acc; + } + } + + /* Calculate jerk */ + if (time > duration) { + if (impl->use_position_control) { + double pos_error = fabs(*pos - impl->target_pos); + double vel_error = fabs(*vel - impl->target_vel); + double acc_threshold = 1e-6; + int acc_near_zero = (fabs(*acc) < acc_threshold); + if (pos_error < TP_POS_EPSILON * 100.0 && vel_error < TP_VEL_EPSILON * 10.0 && acc_near_zero) { + *jerk = 0.0; + *acc = 0.0; + } + } else { + double vel_error = fabs(*vel - impl->target_vel); + double acc_threshold = 1e-6; + int acc_near_zero = (fabs(*acc) < acc_threshold); + if (vel_error < TP_VEL_EPSILON * 10.0 && acc_near_zero) { + *jerk = 0.0; + *acc = 0.0; + } + } + } else if (query_time > impl->cycle_time) { + /* Compute jerk from acceleration difference */ + double prev_pos, prev_vel, prev_acc_val, prev_jerk_unused; + size_t prev_section; + double prev_time = query_time - impl->cycle_time; + if (prev_time < 0.0) prev_time = 0.0; + cruckig_trajectory_at_time(impl->trajectory, prev_time, + &prev_pos, &prev_vel, &prev_acc_val, &prev_jerk_unused, + &prev_section); + *jerk = (new_acc - prev_acc_val) / impl->cycle_time; + } else { + /* First cycle after replanning */ + if (impl->is_first_cycle) { + double base_acc = impl->last_actual_acc; + *jerk = (new_acc - base_acc) / impl->cycle_time; + impl->is_first_cycle = 0; + } else { + /* Use initial acceleration from planning time */ + *jerk = (query_time > 0.0) ? + (new_acc - impl->input->current_acceleration[0]) / query_time : 0.0; + } + } + + /* Save current acceleration for jerk calculation in next cycle */ + impl->last_actual_acc = *acc; + + return 0; +} + +int ruckig_next_cycle(RuckigPlanner planner, + double current_time, + double cycle_time, + double *pos, + double *vel, + double *acc, + double *jerk) { + if (!planner) { + return -1; + } + + double next_time = current_time + cycle_time; + return ruckig_at_time(planner, next_time, pos, vel, acc, jerk); +} + +double ruckig_get_duration(RuckigPlanner planner) { + if (!planner) { + return -1.0; + } + + struct RuckigPlannerImpl *impl = (struct RuckigPlannerImpl *)planner; + + if (!impl->planned) { + return -1.0; + } + + return cruckig_trajectory_get_duration(impl->trajectory); +} + +int ruckig_is_finished(RuckigPlanner planner, double current_time) { + if (!planner) { + return -1; + } + + struct RuckigPlannerImpl *impl = (struct RuckigPlannerImpl *)planner; + + if (!impl->planned) { + return -1; + } + + double duration = ruckig_get_duration(planner); + if (duration < 0.0) { + return -1; + } + + return (current_time >= duration) ? 1 : 0; +} + +void ruckig_reset(RuckigPlanner planner) { + if (!planner) { + return; + } + + struct RuckigPlannerImpl *impl = (struct RuckigPlannerImpl *)planner; + + /* Reset all state fields */ + impl->planned = 0; + impl->start_time = 0.0; + impl->target_pos = 0.0; + impl->target_vel = 0.0; + impl->target_acc = 0.0; + impl->use_position_control = 0; + impl->last_actual_acc = 0.0; + impl->is_first_cycle = 0; + /* Note: do not reset enable_logging, preserve user setting */ + + /* Reset cruckig objects */ + cruckig_reset(impl->otg); +} + +void ruckig_set_logging(RuckigPlanner planner, int enable) { + if (!planner) { + return; + } + + struct RuckigPlannerImpl *impl = (struct RuckigPlannerImpl *)planner; + impl->enable_logging = (enable != 0) ? 1 : 0; +} + +int ruckig_get_decelerate_phases(RuckigPlanner planner, double *t1, double *t2) { + if (!planner) { + return -1; + } + + struct RuckigPlannerImpl *impl = (struct RuckigPlannerImpl *)planner; + + if (!impl->planned) { + rtapi_print_msg(RTAPI_MSG_ERR, "ruckig_get_decelerate_phases: trajectory not planned\n"); + return -1; + } + + /* Get Profile (1 DOF, section 0) */ + const CRuckigProfile *profile = cruckig_trajectory_get_profile(impl->trajectory, 0); + if (!profile) { + rtapi_print_msg(RTAPI_MSG_ERR, "ruckig_get_decelerate_phases: no profile available\n"); + return -1; + } + + /* Deceleration phases: t[4]=T1 (jerk), t[5]=T2 (constant accel) */ + if (t1 != NULL) { + *t1 = (profile->t[4] > 0.0) ? profile->t[4] : 0.0; + } + if (t2 != NULL) { + *t2 = (profile->t[5] > 0.0) ? profile->t[5] : 0.0; + } + + return 0; +} + +int ruckig_get_peak_velocity(RuckigPlanner planner, double *peak_vel) { + if (!planner || !peak_vel) { + return -1; + } + + struct RuckigPlannerImpl *impl = (struct RuckigPlannerImpl *)planner; + + if (!impl->planned) { + rtapi_print_msg(RTAPI_MSG_ERR, "ruckig_get_peak_velocity: trajectory not planned\n"); + return -1; + } + + const CRuckigProfile *profile = cruckig_trajectory_get_profile(impl->trajectory, 0); + if (!profile) { + rtapi_print_msg(RTAPI_MSG_ERR, "ruckig_get_peak_velocity: no profile available\n"); + return -1; + } + + /* Peak velocity is the maximum of v[0] through v[7] */ + double max_v = 0.0; + size_t i; + for (i = 0; i < 8; i++) { + if (profile->v[i] > max_v) { + max_v = profile->v[i]; + } + } + + *peak_vel = max_v; + return 0; +} + +int ruckig_get_start_velocity(RuckigPlanner planner, double *start_vel) { + if (!planner || !start_vel) { + return -1; + } + + struct RuckigPlannerImpl *impl = (struct RuckigPlannerImpl *)planner; + + if (!impl->planned) { + rtapi_print_msg(RTAPI_MSG_ERR, "ruckig_get_start_velocity: trajectory not planned\n"); + return -1; + } + + const CRuckigProfile *profile = cruckig_trajectory_get_profile(impl->trajectory, 0); + if (!profile) { + rtapi_print_msg(RTAPI_MSG_ERR, "ruckig_get_start_velocity: no profile available\n"); + return -1; + } + + *start_vel = profile->v[0]; + return 0; +} + +int ruckig_get_time_at_position(RuckigPlanner planner, double position, double time_after, double *time) { + if (!planner || time == NULL) { + return -1; + } + + struct RuckigPlannerImpl *impl = (struct RuckigPlannerImpl *)planner; + + if (!impl->planned) { + rtapi_print_msg(RTAPI_MSG_ERR, "ruckig_get_time_at_position: trajectory not planned\n"); + return -1; + } + + double result_time; + if (cruckig_trajectory_get_first_time_at_position(impl->trajectory, 0, position, + &result_time, time_after)) { + *time = result_time; + return 0; + } else { + return -1; + } +} diff --git a/wasm-port/vendor/linuxcnc/src/emc/tp/ruckig_wrapper.h b/wasm-port/vendor/linuxcnc/src/emc/tp/ruckig_wrapper.h new file mode 100644 index 0000000..7904249 --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/tp/ruckig_wrapper.h @@ -0,0 +1,221 @@ +/******************************************************************** +* Description: ruckig_wrapper.h +* Ruckig trajectory planning library wrapper for LinuxCNC +* +* This wrapper provides a C interface to Ruckig C++ library +* for S-curve trajectory planning. +* +* License: GPL Version 2 +* System: Linux +* +* Copyright (c) 2024 All rights reserved. +********************************************************************/ +#ifndef RUCKIG_WRAPPER_H +#define RUCKIG_WRAPPER_H + +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * Ruckig trajectory planner handle (opaque pointer) + */ +typedef void* RuckigPlanner; + +/** + * Create a Ruckig trajectory planner. + * + * @param cycle_time cycle time in seconds + * @return planner handle, or NULL on failure + */ +RuckigPlanner ruckig_create(double cycle_time); + +/** + * Destroy a Ruckig trajectory planner. + * + * @param planner planner handle + */ +void ruckig_destroy(RuckigPlanner planner); + +/** + * Plan an S-curve trajectory in position control mode. + * + * Given the initial and target states, plan a complete S-curve trajectory. + * + * @param planner planner handle + * @param current_pos current position + * @param current_vel current velocity + * @param current_acc current acceleration + * @param target_pos target position + * @param target_vel target velocity (usually 0) + * @param target_acc target acceleration (usually 0) + * @param min_vel minimum velocity limit (set to 0 for unidirectional motion) + * @param max_vel maximum velocity limit + * @param max_acc maximum acceleration limit + * @param max_jerk maximum jerk limit + * @return 0 on success, -1 on failure (insufficient distance or invalid params) + */ +int ruckig_plan_position(RuckigPlanner planner, + double current_pos, + double current_vel, + double current_acc, + double target_pos, + double target_vel, + double target_acc, + double min_vel, + double max_vel, + double max_acc, + double max_jerk); + +/** + * Plan an S-curve trajectory in velocity control mode (for stop/pause). + * + * Uses velocity control mode, ignoring target position. + * Suitable for stop or pause scenarios where deceleration may span segments. + * + * @param planner planner handle + * @param current_vel current velocity + * @param current_acc current acceleration + * @param target_vel target velocity (0 for stop) + * @param target_acc target acceleration (usually 0) + * @param min_vel minimum velocity limit (set to 0 for unidirectional motion) + * @param max_acc maximum acceleration limit + * @param max_jerk maximum jerk limit + * @return 0 on success, -1 on failure (invalid params) + */ +int ruckig_plan_velocity(RuckigPlanner planner, + double current_vel, + double current_acc, + double target_vel, + double target_acc, + double min_vel, + double max_acc, + double max_jerk); + +/** + * Get the motion state at a specified time. + * + * @param planner planner handle + * @param time time in seconds (from trajectory start) + * @param pos [out] position + * @param vel [out] velocity + * @param acc [out] acceleration + * @param jerk [out] jerk + * @return 0 on success, -1 on failure (time out of range) + */ +int ruckig_at_time(RuckigPlanner planner, + double time, + double *pos, + double *vel, + double *acc, + double *jerk); + +/** + * Get the motion state at the next cycle. + * + * Computes the state at (current_time + cycle_time). + * + * @param planner planner handle + * @param current_time current time in seconds (from trajectory start) + * @param cycle_time cycle time in seconds + * @param pos [out] position + * @param vel [out] velocity + * @param acc [out] acceleration + * @param jerk [out] jerk + * @return 0 on success, -1 on failure (time out of range or not planned) + */ +int ruckig_next_cycle(RuckigPlanner planner, + double current_time, + double cycle_time, + double *pos, + double *vel, + double *acc, + double *jerk); + +/** + * Get total trajectory duration. + * + * @param planner planner handle + * @return total time in seconds, or -1.0 on failure + */ +double ruckig_get_duration(RuckigPlanner planner); + +/** + * Check if the trajectory has completed. + * + * @param planner planner handle + * @param current_time current time in seconds + * @return 1 if finished, 0 if not, -1 on error + */ +int ruckig_is_finished(RuckigPlanner planner, double current_time); + +/** + * Reset the planner state. + * + * Clears previous planning results, preparing for new planning. + * + * @param planner planner handle + */ +void ruckig_reset(RuckigPlanner planner); + +/** + * Enable or disable log output. + * + * Controls whether the planner outputs error and warning messages. + * For velocity planning scenarios (e.g. sp_scurve.c), logging can be + * disabled to avoid unnecessary warnings. + * + * @param planner planner handle + * @param enable 1=enable logging, 0=disable logging + */ +void ruckig_set_logging(RuckigPlanner planner, int enable); + +/** + * Get the deceleration phase durations (T1 and T2) from the Ruckig profile. + * + * T1: time for acceleration to change from 0 to -amax (jerk phase) + * T2: time at constant -amax acceleration (constant accel phase) + * + * @param planner planner handle (must have completed planning) + * @param t1 [out] T1 time (jerk phase), NULL if not needed + * @param t2 [out] T2 time (constant accel phase), NULL if not needed + * @return 0 on success, -1 on failure (not planned or cannot retrieve) + */ +int ruckig_get_decelerate_phases(RuckigPlanner planner, double *t1, double *t2); + +/** + * Get the peak velocity of the trajectory. + * + * @param planner planner handle (must have completed planning) + * @param peak_vel [out] peak velocity + * @return 0 on success, -1 on failure (not planned or cannot retrieve) + */ +int ruckig_get_peak_velocity(RuckigPlanner planner, double *peak_vel); + +/** + * Get the start velocity of the trajectory. + * + * @param planner planner handle (must have completed planning) + * @param start_vel [out] start velocity + * @return 0 on success, -1 on failure (not planned or cannot retrieve) + */ +int ruckig_get_start_velocity(RuckigPlanner planner, double *start_vel); + +/** + * Get the time at which the trajectory first reaches a given position. + * + * @param planner planner handle (must have completed planning) + * @param position target position + * @param time_after start query time (optional, default 0.0) + * @param time [out] time at which position is reached + * @return 0 on success, -1 on failure (not planned, position unreachable) + */ +int ruckig_get_time_at_position(RuckigPlanner planner, double position, double time_after, double *time); + +#ifdef __cplusplus +} +#endif + +#endif /* RUCKIG_WRAPPER_H */ diff --git a/wasm-port/vendor/linuxcnc/src/emc/tp/sp_scurve.c b/wasm-port/vendor/linuxcnc/src/emc/tp/sp_scurve.c new file mode 100644 index 0000000..d72caad --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/tp/sp_scurve.c @@ -0,0 +1,653 @@ +/*! +******************************************************************** +* Description: sp_scurve.c +*\brief Ruckig-based S-curve trajectory planning with legacy helpers +* +*\author Derived from a work by Yang Yang +* +* License: GPL Version 2 +* System: Linux +* +* Copyright (c) 2004 All rights reserved. +* +* Last change: +********************************************************************/ +#include +#include + +#include "sp_scurve.h" +#include "tp_types.h" +#include "ruckig_wrapper.h" + +#ifndef __KERNEL__ +#include +#include +#endif + +/* ========== Cached Ruckig planner ========== + * Use a static variable to cache the planner, avoiding creation and + * destruction on every call. + */ +static RuckigPlanner cached_planner = NULL; +static double cached_cycle_time = 0.0; /* cycle time used by the current planner */ + +/** + * @brief Initialize the S-curve planner (call at program entry). + * + * @param cycle_time cycle time in seconds + * @return 0 on success, -1 on failure + */ +int sp_scurve_init(double cycle_time) { + /* Parameter validation */ + if (cycle_time <= 0.0) { + rtapi_print_msg(RTAPI_MSG_ERR, "sp_scurve_init: invalid cycle_time=%f\n", cycle_time); + return -1; + } + + /* If planner already exists with the same cycle time, nothing to do */ + if (cached_planner != NULL && fabs(cached_cycle_time - cycle_time) < 1e-12) { + return 0; + } + + /* If planner exists but cycle time changed, destroy the old one first */ + if (cached_planner != NULL) { + rtapi_print_msg(RTAPI_MSG_INFO, "sp_scurve_init: cycle time changed from %f to %f, recreating planner\n", + cached_cycle_time, cycle_time); + ruckig_destroy(cached_planner); + cached_planner = NULL; + } + + /* Create new planner */ + cached_planner = ruckig_create(cycle_time); + if (cached_planner == NULL) { + rtapi_print_msg(RTAPI_MSG_ERR, "sp_scurve_init: ruckig_create() failed with cycle_time=%f\n", cycle_time); + return -1; + } + + /* Disable log output (used for velocity planning — avoids unnecessary warnings) */ + ruckig_set_logging(cached_planner, 0); + + cached_cycle_time = cycle_time; + rtapi_print_msg(RTAPI_MSG_INFO, "sp_scurve_init: planner created with cycle_time=%f (logging disabled)\n", cycle_time); + return 0; +} + +/** + * @brief Clean up the S-curve planner (call at program exit). + */ +void sp_scurve_cleanup(void) { + if (cached_planner != NULL) { + ruckig_destroy(cached_planner); + cached_planner = NULL; + cached_cycle_time = 0.0; + } +} + +/** + * @brief Get the cached Ruckig planner. + * + * Note: sp_scurve_init() must be called before using this. + * + * @return RuckigPlanner handle, or NULL if not initialized + */ +static RuckigPlanner get_cached_planner(void) { + /* If planner is not initialized, return NULL. + * Callers should check the return value and handle the error. */ + return cached_planner; +} + +/* ================================================================ + * Ruckig-based S-curve functions + * ================================================================ */ + +/** + * @brief Compute the S-curve peak velocity from rest to end-speed + * (using Ruckig planning). + * + * Given total distance and end velocity, plan a complete trajectory + * from (0, 0, 0) to (distance, Ve, 0), then read the peak velocity + * directly from the profile — no iteration required. + * + * @param distance total distance + * @param Ve end velocity + * @param maxA maximum acceleration + * @param maxJ maximum jerk + * @param req_v [out] computed peak velocity + * @return 1 on success, -1 on failure + */ +int findSCurveVSpeedWithEndSpeed(double distance, double Ve, + double maxA, double maxJ, double* req_v) { + /* Parameter validation */ + if (distance <= 0 || maxA <= 0 || maxJ <= 0) { + *req_v = fabs(Ve); + return -1; + } + + /* When Ve is approximately zero, use the symmetric function */ + if (fabs(Ve) <= TP_VEL_EPSILON) { + return findSCurveVSpeed(distance, maxA, maxJ, req_v); + } + + /* Use the cached planner */ + RuckigPlanner planner = get_cached_planner(); + if (!planner) { + rtapi_print_msg(RTAPI_MSG_ERR, "findSCurveVSpeedWithEndSpeed: planner not initialized, call sp_scurve_init() first\n"); + *req_v = fabs(Ve); + return -1; + } + + /* Reset planner state */ + ruckig_reset(planner); + + /* Plan a complete trajectory from (0, 0, 0) to (distance, Ve, 0). + * Ruckig will automatically find the peak velocity that satisfies + * the distance and end-velocity constraints. */ + int result = ruckig_plan_position(planner, + 0.0, /* start position */ + 0.0, /* start velocity */ + 0.0, /* start acceleration */ + distance, /* target position */ + Ve, /* target velocity */ + 0.0, /* target acceleration */ + 0.0, /* min velocity (unidirectional) */ + sqrt(maxA * distance + Ve * Ve) * 2.0, /* max velocity (conservative, ensures no limiting) */ + maxA, /* max acceleration */ + maxJ); /* max jerk */ + + if (result != 0) { + rtapi_print_msg(RTAPI_MSG_ERR, "findSCurveVSpeedWithEndSpeed: ruckig_plan_position failed (result=%d)\n", result); + *req_v = fabs(Ve); + return -1; + } + + /* Read the peak velocity directly from the profile */ + double peak_vel = 0.0; + result = ruckig_get_peak_velocity(planner, &peak_vel); + if (result != 0) { + rtapi_print_msg(RTAPI_MSG_ERR, "findSCurveVSpeedWithEndSpeed: ruckig_get_peak_velocity failed\n"); + *req_v = fabs(Ve); + return -1; + } + + *req_v = peak_vel; + return 1; +} + +/** + * @brief Compute the maximum start speed that can decelerate to Ve within + * a given distance (jerk-constrained). + * + * Find the largest Vs such that a trajectory exists from (0, Vs, 0) to + * (distance, Ve, 0) under (maxA, maxJ) constraints. + * + * Method: use the constant-acceleration upper bound + * Vs_estimate = sqrt(Ve^2 + 2*maxA*distance) + * as an initial guess and pass it to Ruckig. If planning succeeds, + * Vs_estimate is feasible. If it fails, the jerk constraint requires + * more distance — return a guaranteed-feasible upper bound instead. + * + * On failure, instead of returning 0.9*Vs_estimate (which may still + * exceed the jerk-feasible value), return the 0->0 S-curve peak for + * the same distance. That value is always jerk-feasible and prevents + * downstream planning failures. On success the same peak is used as + * an upper-bound clamp. + * + * @param distance total distance + * @param Ve end velocity + * @param maxA maximum acceleration + * @param maxJ maximum jerk + * @param req_v [out] computed maximum start speed + * @return 1 on success, -1 on failure + */ +int findSCurveMaxStartSpeed(double distance, double Ve, + double maxA, double maxJ, double* req_v) { + if (distance <= 0 || maxA <= 0 || maxJ <= 0) { + *req_v = fabs(Ve); + return -1; + } + + if (fabs(Ve) <= TP_VEL_EPSILON) { + return findSCurveVSpeed(distance, maxA, maxJ, req_v); + } + + /* 0->0 S-curve peak for this distance — reliable jerk-constrained upper bound, + * used as fallback on failure and as a clamp on success. */ + double v_0_to_0_peak = 0.0; + if (findSCurveVSpeed(distance, maxA, maxJ, &v_0_to_0_peak) != 1) { + /* findSCurveVSpeed failed: use triangular upper bound to avoid unbounded result */ + v_0_to_0_peak = sqrt(maxA * distance); + } + + RuckigPlanner planner = get_cached_planner(); + if (!planner) { + rtapi_print_msg(RTAPI_MSG_ERR, "findSCurveMaxStartSpeed: planner not initialized, call sp_scurve_init() first\n"); + *req_v = fmin(fabs(Ve) * 2.0, v_0_to_0_peak); + return -1; + } + + ruckig_reset(planner); + + double Vs_estimate = sqrt(Ve * Ve + 2.0 * maxA * distance); + if (Vs_estimate < fabs(Ve)) { + Vs_estimate = fabs(Ve) * 2.0; + } + + int result = ruckig_plan_position(planner, + 0.0, + Vs_estimate, + 0.0, + distance, + Ve, + 0.0, + 0.0, + Vs_estimate * 2.0, + maxA, + maxJ); + + if (result == 0) { + double duration = ruckig_get_duration(planner); + if (duration > 0.0) { + double actual_pos, actual_vel, actual_acc, actual_jerk; + int query_result = ruckig_at_time(planner, duration, + &actual_pos, &actual_vel, + &actual_acc, &actual_jerk); + if (query_result == 0) { + double pos_error = fabs(actual_pos - distance); + if (pos_error < 1e-6) { + double start_vel = 0.0; + if (ruckig_get_start_velocity(planner, &start_vel) == 0) { + *req_v = fmin(start_vel, v_0_to_0_peak); + return 1; + } + } + } + } + *req_v = fmin(Vs_estimate, v_0_to_0_peak); + return 1; + } + + /* Planning failed: jerk constraint makes Vs_estimate infeasible. + * Return the guaranteed-feasible 0->0 peak to avoid downstream failures. */ + *req_v = fmax(fabs(Ve), v_0_to_0_peak); + return 1; +} + +/** + * @brief Compute the rest-to-rest S-curve peak velocity (using Ruckig planning). + * + * Given a total distance, plan a complete trajectory from (0, 0, 0) to + * (distance, 0, 0), then read the peak velocity directly from the + * profile — no iteration required. + * + * @param distence total distance (rest to rest) + * @param maxA maximum acceleration + * @param maxJ maximum jerk + * @param req_v [out] computed peak velocity + * @return 1 on success, -1 on failure + */ +int findSCurveVSpeed(double distence, double maxA, double maxJ, double* req_v){ + /* Parameter validation */ + if (distence <= 0 || maxA <= 0 || maxJ <= 0) { + *req_v = 0.0; + return -1; + } + + /* Use the cached planner */ + RuckigPlanner planner = get_cached_planner(); + if (!planner) { + rtapi_print_msg(RTAPI_MSG_ERR, "findSCurveVSpeed: planner not initialized, call sp_scurve_init() first\n"); + *req_v = 0.0; + return -1; + } + + /* Reset planner state */ + ruckig_reset(planner); + + /* Plan a complete trajectory from (0, 0, 0) to (distance, 0, 0) */ + int result = ruckig_plan_position(planner, + 0.0, /* start position */ + 0.0, /* start velocity */ + 0.0, /* start acceleration */ + distence, /* target position */ + 0.0, /* target velocity */ + 0.0, /* target acceleration */ + 0.0, /* min velocity (unidirectional) */ + sqrt(maxA * distence) * 2.0, /* max velocity (conservative, ensures no limiting) */ + maxA, /* max acceleration */ + maxJ); /* max jerk */ + + if (result != 0) { + rtapi_print_msg(RTAPI_MSG_ERR, "findSCurveVSpeed: ruckig_plan_position failed (result=%d)\n", result); + *req_v = 0.0; + return -1; + } + + /* Read the peak velocity directly from the profile */ + double peak_vel = 0.0; + result = ruckig_get_peak_velocity(planner, &peak_vel); + if (result != 0) { + rtapi_print_msg(RTAPI_MSG_ERR, "findSCurveVSpeed: ruckig_get_peak_velocity failed\n"); + *req_v = 0.0; + return -1; + } + + *req_v = peak_vel; + return 1; +} + +/** + * @brief Compute S-curve deceleration time parameters using analytical formulas + * (real-time optimized version). + * + * S-curve deceleration consists of three phases: + * T1: jerk ramp-up phase (j = -jerk), acceleration goes from 0 to -amax + * T2: constant deceleration phase (j = 0), acceleration stays at -amax + * T1: jerk ramp-down phase (j = +jerk), acceleration goes from -amax to 0 + * + * ========== Velocity-time curve ========== + * + * velocity v + * ^ + * V |------\ + * | \ + * | \____ + * | \ + * | \ + * +---------------\----> time t + * 0 T1 T1+T2 2T1+T2 + * + * ========== Analytical formula derivation ========== + * + * For S-curve deceleration: + * - T1 = amax / jerk (time for acceleration to go from 0 to -amax) + * - Phase 1 velocity loss: dv1 = 0.5 * jerk * T1^2 = 0.5 * amax^2 / jerk + * - Phase 3 velocity loss: dv3 = 0.5 * jerk * T1^2 = 0.5 * amax^2 / jerk (same as phase 1) + * - Phase 2 velocity loss: dv2 = amax * T2 + * - Total velocity loss: v = dv1 + dv2 + dv3 = amax^2 / jerk + amax * T2 + * - Therefore: T2 = (v - amax^2 / jerk) / amax + * + * Special case (triangular profile): + * - If v < amax^2 / jerk, the velocity is too small for a full S-curve + * (no constant deceleration phase) + * - For triangular profile: v = jerk * T1^2, so T1 = sqrt(v / jerk), T2 = 0 + * + * ========== Optimization notes ========== + * + * This function uses analytical formulas for direct computation, avoiding + * frequent trajectory planning — suitable for real-time system calls. + * Compared to using Ruckig, performance is significantly better and results + * are fully consistent. + * + * @param v initial velocity (absolute value is taken) + * @param amax maximum acceleration + * @param jerk maximum jerk + * @param t1 [out, optional] jerk phase time T1 + * @param t2 [out, optional] constant deceleration phase time T2 + * @return total deceleration time = 2*T1 + T2 + */ +double calcDecelerateTimes(double v, double amax, double jerk, double* t1, double* t2){ + v = fabs(v); + + /* Parameter validation */ + if (v < TP_VEL_EPSILON) { + if (t1 != NULL) *t1 = 0.0; + if (t2 != NULL) *t2 = 0.0; + return 0.0; + } + + if (amax <= 0.0 || jerk <= 0.0) { + if (t1 != NULL) *t1 = 0.0; + if (t2 != NULL) *t2 = 0.0; + return 0.0; + } + + /* Compute T1 (jerk phase time) */ + double T1 = amax / jerk; + + /* Total velocity loss from phase 1 and phase 3: + * dv1 + dv3 = 2 * (0.5 * amax^2 / jerk) = amax^2 / jerk */ + double v_loss_jerk_phases = amax * amax / jerk; + + double T2 = 0.0; + + /* Determine whether this is a full S-curve or a triangular profile */ + if (v >= v_loss_jerk_phases) { + /* Full S-curve: constant deceleration phase exists */ + T2 = (v - v_loss_jerk_phases) / amax; + if (T2 < 0.0) { + T2 = 0.0; /* guard against numerical error */ + } + } else { + /* Triangular profile: no constant deceleration phase, recompute T1. + * v = jerk * T1^2, so T1 = sqrt(v / jerk) */ + T1 = sqrt(v / jerk); + T2 = 0.0; + } + + /* Output results */ + if (t1 != NULL) *t1 = T1; + if (t2 != NULL) *t2 = T2; + + /* Total time: 2*T1 + T2 + * (T1 to ramp accel to -amax, T2 at constant -amax, T1 to ramp back to 0) */ + return T1 * 2.0 + T2; +} + +/** + * @brief Compute the maximum speed reachable from rest in time T using + * an S-curve profile (via Ruckig planning). + * + * Given maximum acceleration amax, maximum jerk, and time T, compute the + * maximum velocity achievable from rest using an S-curve acceleration + * profile within time T. + * + * Algorithm: use Ruckig position-control mode to plan toward a sufficiently + * large target position (ensuring the target is not reached within time T), + * then sample the velocity at time T. + * + * @param amax maximum acceleration + * @param jerk maximum jerk + * @param T time in seconds + * @return maximum velocity at time T, or 0.0 on failure + */ +double calcSCurveSpeedWithT(double amax, double jerk, double T) { + /* Parameter validation */ + if (amax <= 0.0 || jerk <= 0.0 || T <= 0.0) { + return 0.0; + } + + /* Use the cached planner */ + RuckigPlanner planner = get_cached_planner(); + if (!planner) { + rtapi_print_msg(RTAPI_MSG_ERR, "calcSCurveSpeedWithT: planner not initialized, call sp_scurve_init() first\n"); + return 0.0; + } + + /* Reset planner state */ + ruckig_reset(planner); + + /* Estimate a target position large enough that the trajectory will not + * reach it within time T. Use the trapezoidal formula as a conservative + * estimate: s = 0.5 * amax * T^2. Double it for safety. */ + double target_pos = 0.5 * amax * T * T * 2.0; + + /* Set a max velocity large enough to not be the limiting factor */ + double max_vel = amax * T * 2.0; /* conservative estimate */ + + int result = ruckig_plan_position(planner, + 0.0, /* start position */ + 0.0, /* start velocity */ + 0.0, /* start acceleration */ + target_pos, /* target position (large enough) */ + max_vel, /* target velocity (large, not limiting) */ + 0.0, /* target acceleration */ + 0.0, /* min velocity (unidirectional) */ + max_vel * 2.0, /* max velocity (ensures no limiting) */ + amax, /* max acceleration */ + jerk); /* max jerk */ + + if (result != 0) { + /* Planning failed — use conservative fallback estimate. + * For an S-curve the velocity upper bound at time T is amax*T + * (trapezoidal), but the S-curve value is smaller. */ + return fmin(amax * T, sqrt(amax * amax * T / jerk)); + } + + /* Sample velocity at time T */ + double pos, vel, acc, jerk_val; + result = ruckig_at_time(planner, T, &pos, &vel, &acc, &jerk_val); + if (result != 0) { + /* Sampling failed — use conservative fallback */ + return fmin(amax * T, sqrt(amax * amax * T / jerk)); + } + + return vel; +} + +/* ================================================================ + * Legacy functions kept for simple_tp.c compatibility + * ================================================================ */ + +/* PT = P0 + V0 * T + 0.5 * A0 * T^2 + J * T^3 / 6 + * VT = V0 + A0 * T + J * T^2 / 2 + * AT = A0 + J * T + */ + +double nextAccel(double t, double targetV, double v, double a, double maxA, + double maxJ) { + double max_da, tiny_da, vel_err, acc_req; + max_da = delta_accel(t, maxJ); + tiny_da = max_da * t * 0.001; + vel_err = targetV - v; + if (vel_err > tiny_da){ + acc_req = -max_da + + sqrt(2.0 * maxJ * vel_err + max_da * max_da); + }else if (vel_err < -tiny_da){ + acc_req = max_da - + sqrt(-2.0 * maxJ * vel_err + max_da * max_da); + }else{ + /* within 'tiny_da' of desired velocity, no need to move */ + acc_req = 0.0; + } + /* limit acceleration request */ + if (acc_req > maxA){ + acc_req = maxA; + }else if (acc_req < -maxA){ + acc_req = -maxA; + } + /* ramp acceleration toward request at jerk limit */ + if (acc_req > a + max_da){ + return a + max_da; + }else if (acc_req < a - max_da){ + return a - max_da; + }else{ + return acc_req; + } +} + +/* PT = P0 + V0 * T + 0.5 * A0 * T^2 + J * T^3 / 6 + * VT = V0 + A0 * T + J * T^2 / 2 + * AT = A0 + J * T + */ +double nextSpeed(double v, double a, double t, double targetV, double maxA, double maxJ, double* req_v, double* req_a, double* req_j) { + /* Compute next acceleration */ + double nextA = nextAccel(t, targetV, v, a, maxA, maxJ); + + /* Compute next velocity using trapezoidal rule: + * VT - V0 = (A0 + AT) * T / 2 */ + double deltaV = (a + nextA) * t / 2.0; + if ((deltaV < 0 && targetV < v && v + deltaV < targetV) || + (0 < deltaV && v < targetV && targetV < v + deltaV)) { + /* Would overshoot target velocity — clamp */ + nextA = 2.0 * (targetV - v) / t - a; + if(nextA >= maxA){ + nextA = maxA; + targetV = (a + nextA) * t / 2.0; + } + v = targetV; + } else { + v += deltaV; + } + + /* Compute jerk = delta accel / time */ + *req_j = (nextA - a) / t; + if(*req_j > maxJ){ + *req_j = maxJ; + nextA = a + maxJ * t; + } else if (*req_j < -maxJ) { + *req_j = -maxJ; + nextA = a - maxJ * t; + } + *req_a = nextA; + *req_v = v; + + return v; +} + +double stoppingDist(double v, double a, double maxA, double maxJ) { + /* Already stopped */ + if (fabs(v) < 0.0001) return 0; + /* Handle negative velocity */ + if (v < 0) { + v = -v; + a = -a; + } + + double d = 0; + + /* Compute distance and velocity change to bring acceleration to 0 */ + if (0 < a) { + double t = a / maxJ; + d += sc_distance(t, v, a, -maxJ); + v += delta_velocity(t, a, -maxJ); + a = 0; + } + + /* Compute maximum deceleration. + * + * At target velocity, both velocity and acceleration are 0. + * VT = 0 + 0*T1 + J*T1^2/2, and because Amax = J*T1: + * VT = Amax^2 / (2*J) + * From the other side: VT = v + (a + Amax)*T2/2 + * Combining: Amax^2 = v*J + 0.5*a*a + */ + double maxDeccel = -sqrt(v * maxJ + 0.5 * a * a); + if (maxDeccel < -maxA) maxDeccel = -maxA; + + /* Compute distance and velocity change to reach max deceleration */ + if (maxDeccel < a) { + double t = (a - maxDeccel) / maxJ; + d += sc_distance(t, v, a, -maxJ); + v += delta_velocity(t, a, -maxJ); + a = maxDeccel; + } + + /* Velocity remaining when entering final jerk phase: + * VT = Amax^2 / (2*J) */ + double deltaV = 0.5 * a * a / maxJ; + + /* Constant deceleration phase (if needed) */ + if (deltaV < v) { + double t = (v - deltaV) / -a; + d += sc_distance(t, v, a, 0); + v += delta_velocity(t, a, 0); + } + + /* Distance to zero velocity (final jerk phase) */ + d += sc_distance(-a / maxJ, v, a, maxJ); + + return d; +} + +/* S-curve displacement: P = v*t + (1/2)*a*t^2 + (1/6)*j*t^3 */ +double sc_distance(double t, double v, double a, double j) { + return t * (v + t * (0.5 * a + 1.0 / 6.0 * j * t)); +} + +/* Velocity change: dV = a*t + (1/2)*j*t^2 */ +double delta_velocity(double t, double a, double j) { + return t * (a + 0.5 * j * t); +} + +/* Acceleration change: dA = j*t */ +double delta_accel(double t, double j) {return j * t;} diff --git a/wasm-port/vendor/linuxcnc/src/emc/tp/sp_scurve.h b/wasm-port/vendor/linuxcnc/src/emc/tp/sp_scurve.h new file mode 100644 index 0000000..80b6dca --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/tp/sp_scurve.h @@ -0,0 +1,60 @@ +/******************************************************************** +* Description: sp_scurve.h +* Discriminate-based trajectory planning +* +* Derived from a work by Yang Yang +* +* Author: Yang Yang +* Contact: mika-net@outlook.com +* License: GPL Version 2 +* System: Linux +* +* Copyright (c) 2004 All rights reserved. +* +* Last change: +********************************************************************/ +#ifndef SP_SCURVE_H +#define SP_SCURVE_H + +#include +#include "../motion/simple_tp.h" + +/** + * Initialize the S-curve planner (call at program entry). + * + * @param cycle_time cycle time in seconds + * @return 0 on success, -1 on failure + */ +int sp_scurve_init(double cycle_time); + +/** + * Clean up the S-curve planner (call at program exit). + */ +void sp_scurve_cleanup(void); + +/* Legacy functions kept for simple_tp.c compatibility */ +double nextAccel(double t, double targetV, double v, double a, double maxA, double maxJ); +double sc_distance(double t, double v, double a, double j); +double delta_velocity(double t, double a, double j); +double delta_accel(double t, double j); +double nextSpeed(double v, double a, double t, double targetV, double maxA, double maxJ, double* req_v, double* req_a, double* req_j); +double stoppingDist(double v, double a, double maxA, double maxJ); + +int findSCurveVSpeed(double distence,/* double maxV, */double maxA, double maxJ, double *req_v); +int findSCurveVSpeedWithEndSpeed(double distence, double Ve, double maxA, double maxJ, double* req_v); +int findSCurveMaxStartSpeed(double distance, double Ve, double maxA, double maxJ, double* req_v); +double calcDecelerateTimes(double v, double amax, double jerk, double* t1, double* t2); +double calcSCurveSpeedWithT(double amax, double jerk, double T); + +/** + * tpCalculateSCurveAccel return value definitions + * + * TP_SCURVE_ACCEL_ERROR - calculation failed (maxjerk invalid or less than/equal to 1) + * TP_SCURVE_ACCEL_ACCEL - acceleration or normal state (no deceleration needed) + * TP_SCURVE_ACCEL_DECEL - deceleration needed + */ +#define TP_SCURVE_ACCEL_ERROR -5 +#define TP_SCURVE_ACCEL_ACCEL 0 +#define TP_SCURVE_ACCEL_DECEL 1 + +#endif diff --git a/wasm-port/vendor/linuxcnc/src/emc/tp/spherical_arc.c b/wasm-port/vendor/linuxcnc/src/emc/tp/spherical_arc.c new file mode 100644 index 0000000..2c46279 --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/tp/spherical_arc.c @@ -0,0 +1,202 @@ +/******************************************************************** + * Description: spherical_arc.c + * + * A simple spherical linear interpolation library and related functions. + * + * Author: Robert W. Ellenberg + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2014 All rights reserved. + * + ********************************************************************/ + +#include + +#include "spherical_arc.h" +#include "tp_types.h" + +#include "tp_debug.h" + +int arcInitFromPoints(SphericalArc * const arc, PmCartesian const * const start, + PmCartesian const * const end, + PmCartesian const * const center) +{ +#ifdef ARC_PEDANTIC + if (!P0 || !P1 || !center) + return TP_ERR_MISSING_INPUT; + + if (!arc) + return TP_ERR_MISSING_OUTPUT; +#endif + + // Store the start, end, and center + arc->start = *start; + arc->end = *end; + arc->center = *center; + + pmCartCartSub(start, center, &arc->rStart); + pmCartCartSub(end, center, &arc->rEnd); + + // Find the radii at start and end. These are identical for a perfect spherical arc + double radius0, radius1; + pmCartMag(&arc->rStart, &radius0); + pmCartMag(&arc->rEnd, &radius1); + + tp_debug_print("radii are %g and %g\n", + radius0, + radius1); + + if (radius0 < ARC_MIN_RADIUS || radius1 < ARC_MIN_RADIUS) { + tp_debug_print("radius below min radius %f, aborting arc\n", + ARC_MIN_RADIUS); + return TP_ERR_RADIUS_TOO_SMALL; + } + + // Choose initial radius as nominal radius + arc->radius = radius0; + + // Get unit vectors from center to start and center to end + PmCartesian u0, u1; + pmCartScalMult(&arc->rStart, 1.0 / radius0, &u0); + pmCartScalMult(&arc->rEnd, 1.0 / radius1, &u1); + + // Find arc angle + double dot; + pmCartCartDot(&u0, &u1, &dot); + arc->angle = acos(dot); + tp_debug_print("spherical arc angle = %f\n", arc->angle); + + // Store spiral factor as radial difference. Archimedean spiral coef. a = spiral / angle + arc->spiral = (radius1 - radius0 ); + + if (arc->angle < ARC_MIN_ANGLE) { + tp_debug_print("angle %f below min angle %f, aborting arc\n", + arc->angle, + ARC_MIN_ANGLE); + return TP_ERR_GEOM; + } + + // Store sin of arc angle since it is reused many times for SLERP + arc->Sangle = sin(arc->angle); + + return TP_ERR_OK; +} + +int arcPoint(SphericalArc const * const arc, double progress, PmCartesian * const out) +{ + //TODO pedantic + + //Convert progress to actual progress around the arc + double net_progress = progress - arc->line_length; + if (net_progress <= 0.0 && arc->line_length > 0) { + tc_debug_print("net_progress = %f, line_length = %f\n", net_progress, arc->line_length); + //Get position on line (not actually an angle in this case) + pmCartScalMult(&arc->uTan, net_progress, out); + pmCartCartAdd(out, &arc->start, out); + } else { + double angle_in = net_progress / arc->radius; + tc_debug_print("angle_in = %f, angle_total = %f\n", angle_in, arc->angle); + double scale0 = sin(arc->angle - angle_in) / arc->Sangle; + double scale1 = sin(angle_in) / arc->Sangle; + + PmCartesian interp0,interp1; + pmCartScalMult(&arc->rStart, scale0, &interp0); + pmCartScalMult(&arc->rEnd, scale1, &interp1); + + pmCartCartAdd(&interp0, &interp1, out); + pmCartCartAdd(&arc->center, out, out); + } + return TP_ERR_OK; +} + +int arcLength(SphericalArc const * const arc, double * const length) +{ + *length = arc->radius * arc->angle + arc->line_length; + tp_debug_print("arc length = %g\n", *length); + return TP_ERR_OK; +} + +int arcFromLines(SphericalArc * const arc, PmCartLine const * const line1, + PmCartLine const * const line2, double radius, + double blend_dist, double center_dist, PmCartesian * const start, PmCartesian * const end, int consume) { + (void)radius; + + PmCartesian center, normal, binormal; + + // Pointer to middle point of line segment pair + PmCartesian const * const middle = &line1->end; + //TODO assert line1 end = line2 start? + + //Calculate the normal direction of the arc from the difference + //between the unit vectors + pmCartCartSub(&line2->uVec, &line1->uVec, &normal); + pmCartUnitEq(&normal); + pmCartScalMultEq(&normal, center_dist); + pmCartCartAdd(middle, &normal, ¢er); + + //Calculate the binormal (vector perpendicular to the plane of the + //arc) + pmCartCartCross(&line1->uVec, &line2->uVec, &binormal); + pmCartUnitEq(&binormal); + + // Start point is blend_dist away from middle point in the + // negative direction of line1 + pmCartScalMult(&line1->uVec, -blend_dist, start); + pmCartCartAdd(start, middle, start); + + // End point is blend_dist away from middle point in the positive + // direction of line2 + pmCartScalMult(&line2->uVec, blend_dist, end); + pmCartCartAddEq(end, middle); + + //Handle line portion of line-arc + arc->uTan = line1->uVec; + if (consume) { + arc->line_length = line1->tmag - blend_dist; + } else { + arc->line_length = 0; + } + + return arcInitFromPoints(arc, start, end, ¢er); +} + +int arcConvexTest(PmCartesian const * const center, + PmCartesian const * const P, PmCartesian const * const uVec, int reverse_dir) +{ + //Check if an arc-line intersection is concave or convex + double dot; + PmCartesian diff; + pmCartCartSub(P, center, &diff); + pmCartCartDot(&diff, uVec, &dot); + + tp_debug_print("convex test: dot = %f, reverse_dir = %d\n", dot, reverse_dir); + int convex = (reverse_dir != 0) ^ (dot < 0); + return convex; +} + +int arcTangent(SphericalArc const * const arc, PmCartesian * const tan, int at_end) +{ + PmCartesian r_perp; + PmCartesian r_tan; + + if (at_end) { + r_perp = arc->rEnd; + } else { + r_perp = arc->rStart; + } + + pmCartCartCross(&arc->binormal, &r_perp, &r_tan); + //Get spiral component + double dr = arc->spiral / arc->angle; + + //Get perpendicular component due to spiral + PmCartesian d_perp; + pmCartUnit(&r_perp, &d_perp); + pmCartScalMultEq(&d_perp, dr); + //TODO error checks + pmCartCartAdd(&d_perp, &r_tan, tan); + pmCartUnitEq(tan); + + return TP_ERR_OK; +} diff --git a/wasm-port/vendor/linuxcnc/src/emc/tp/spherical_arc.h b/wasm-port/vendor/linuxcnc/src/emc/tp/spherical_arc.h new file mode 100644 index 0000000..acd5c26 --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/tp/spherical_arc.h @@ -0,0 +1,67 @@ +/******************************************************************** + * Description: spherical_arc.h + * + * A simple spherical linear interpolation library and related functions. + * + * Author: Robert W. Ellenberg + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2014 All rights reserved. + * + ********************************************************************/ +#ifndef SPHERICAL_ARC_H +#define SPHERICAL_ARC_H + +#include + +#define ARC_POS_EPSILON 1e-12 +#define ARC_MIN_RADIUS 1e-12 +#define ARC_MIN_ANGLE 1e-6 +//FIXME relate this to cornering acceleration? +#define ARC_ABS_ERR 5e-4 +#define ARC_REL_ERR 5e-4 + +typedef struct { + // Three defining points for the arc + PmCartesian start; + PmCartesian end; + PmCartesian center; + // Relative vectors from center to start and center to end + // These are cached here since they'll be reused during SLERP + PmCartesian rStart; + PmCartesian rEnd; + PmCartesian uTan; /* Tangent vector at start of arc (copied from + prev. tangent line)*/ + PmCartesian binormal; + double radius; + double spiral; + // Angle that the arc encloses + double angle; + double Sangle; + double line_length; +} SphericalArc; + + +int arcInitFromPoints(SphericalArc * const arc, PmCartesian const * const start, + PmCartesian const * const end, PmCartesian const * const center); + +int arcInitFromVectors(SphericalArc * const arc, PmCartesian const * const vec0, + PmCartesian const * const vec1, + PmCartesian const * const center); + +int arcPoint(SphericalArc const * const arc, double angle_in, PmCartesian * const out); + +int arcNormalizedSlerp(SphericalArc const * const arc, double t, PmCartesian * const out); + +int arcLength(SphericalArc const * const arc, double * const length); + +int arcFromLines(SphericalArc * const arc, PmCartLine const * const line1, + PmCartLine const * const line2, double radius, + double blend_dist, double center_dist, PmCartesian * const start, PmCartesian * const end, int consume); + +int arcConvexTest(PmCartesian const * const center, + PmCartesian const * const P, PmCartesian const * const uVec, int reverse_dir); + +int arcTangent(SphericalArc const * const arc, PmCartesian * const tan, int at_end); +#endif diff --git a/wasm-port/vendor/linuxcnc/src/emc/tp/tc.c b/wasm-port/vendor/linuxcnc/src/emc/tp/tc.c new file mode 100644 index 0000000..a2523f2 --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/tp/tc.c @@ -0,0 +1,1109 @@ +/*! +******************************************************************** +* Description: tc.c +*\brief Discriminate-based trajectory planning +* +*\author Derived from a work by Fred Proctor & Will Shackleford +*\author rewritten by Chris Radek +* +* License: GPL Version 2 +* System: Linux +* +* Copyright (c) 2004 All rights reserved. +* +* Last change: +********************************************************************/ + +#include /* rtapi_print_msg */ +#include +#include +#include +#include + +#include "blendmath.h" +#include "tc.h" +#include "tp_types.h" +#include "spherical_arc.h" +#include "../motion/motion.h" +#include "ruckig_wrapper.h" +#include "cruckig/roots.h" + +//Debug output +#include "tp_debug.h" + +// For jerk-limited arc velocity (planner_type 1) +extern emcmot_status_t *emcmotStatus; + +#ifndef GET_TRAJ_PLANNER_TYPE +#define GET_TRAJ_PLANNER_TYPE() (emcmotStatus->planner_type) +#endif + + +double tcGetMaxTargetVel(TC_STRUCT const * const tc, + double max_scale) +{ + double v_max_target; + + switch (tc->synchronized) { + case TC_SYNC_NONE: + // Get maximum reachable velocity from max feed override + v_max_target = tc->reqvel * max_scale; + break; + + case TC_SYNC_VELOCITY: //Fallthrough + max_scale = 1.0; + /* Fallthrough */ + case TC_SYNC_POSITION: + // Assume no spindle override during blend target + default: + v_max_target = tc->maxvel; + break; + } + + // Clip maximum velocity by the segment's own maximum velocity + return fmin(v_max_target, tc->maxvel); +} + +double tcGetOverallMaxAccel(const TC_STRUCT *tc) +{ + // Handle any acceleration reduction due to an approximate-tangent "blend" with the previous or next segment + double a_scale = (1.0 - fmax(tc->kink_accel_reduce, tc->kink_accel_reduce_prev)); + + // Parabolic blending conditions: If the next segment or previous segment + // has a parabolic blend with this one, acceleration is scaled down by 1/2 + // so that the sum of the two does not exceed the maximum. + if (tc->blend_prev || TC_TERM_COND_PARABOLIC == tc->term_cond) { + a_scale *= 0.5; + } + + return tc->maxaccel * a_scale; +} + +/** + * Get acceleration for a tc based on the trajectory planner state. + */ +double tcGetTangentialMaxAccel(TC_STRUCT const * const tc) +{ + double a_scale = tcGetOverallMaxAccel(tc); + + // Reduce allowed tangential acceleration in circular motions to stay + // within overall limits (accounts for centripetal acceleration while + // moving along the circular path). + if (tc->motion_type == TC_CIRCULAR || tc->motion_type == TC_SPHERICAL) { + //Limit acceleration for circular arcs to allow for normal acceleration + a_scale *= tc->acc_ratio_tan; + } + return a_scale; +} + + +int tcSetKinkProperties(TC_STRUCT *prev_tc, TC_STRUCT *tc, double kink_vel, double accel_reduction) +{ + prev_tc->kink_vel = kink_vel; + // + prev_tc->kink_accel_reduce = fmax(accel_reduction, prev_tc->kink_accel_reduce); + tc->kink_accel_reduce_prev = fmax(accel_reduction, tc->kink_accel_reduce_prev); + + return 0; +} + +int tcInitKinkProperties(TC_STRUCT *tc) +{ + tc->kink_vel = -1.0; + tc->kink_accel_reduce = 0.0; + tc->kink_accel_reduce_prev = 0.0; + return 0; +} + +int tcRemoveKinkProperties(TC_STRUCT *prev_tc, TC_STRUCT *tc) +{ + prev_tc->kink_vel = -1.0; + prev_tc->kink_accel_reduce = 0.0; + tc->kink_accel_reduce_prev = 0.0; + return 0; +} + + +int tcCircleStartAccelUnitVector(TC_STRUCT const * const tc, PmCartesian * const out) +{ + PmCartesian startpoint; + PmCartesian radius; + PmCartesian tan, perp; + + pmCirclePoint(&tc->coords.circle.xyz, 0.0, &startpoint); + pmCartCartSub(&startpoint, &tc->coords.circle.xyz.center, &radius); + pmCartCartCross(&tc->coords.circle.xyz.normal, &radius, &tan); + pmCartUnitEq(&tan); + //The unit vector's actual direction is adjusted by the normal + //acceleration here. This unit vector is NOT simply the tangent + //direction. + pmCartCartSub(&tc->coords.circle.xyz.center, &startpoint, &perp); + pmCartUnitEq(&perp); + + pmCartScalMult(&tan, tcGetOverallMaxAccel(tc), &tan); + pmCartScalMultEq(&perp, pmSq(0.5 * tc->reqvel)/tc->coords.circle.xyz.radius); + pmCartCartAdd(&tan, &perp, out); + pmCartUnitEq(out); + return 0; +} + +int tcCircleEndAccelUnitVector(TC_STRUCT const * const tc, PmCartesian * const out) +{ + PmCartesian endpoint; + PmCartesian radius; + + pmCirclePoint(&tc->coords.circle.xyz, tc->coords.circle.xyz.angle, &endpoint); + pmCartCartSub(&endpoint, &tc->coords.circle.xyz.center, &radius); + pmCartCartCross(&tc->coords.circle.xyz.normal, &radius, out); + pmCartUnitEq(out); + return 0; +} + +/** + * Get the acceleration direction unit vector for blend velocity calculations. + * This calculates the direction of acceleration at the start of a segment. + */ +int tcGetStartAccelUnitVector(TC_STRUCT const * const tc, PmCartesian * const out) { + + switch (tc->motion_type) { + case TC_LINEAR: + case TC_RIGIDTAP: + *out=tc->coords.line.xyz.uVec; + break; + case TC_CIRCULAR: + tcCircleStartAccelUnitVector(tc,out); + break; + case TC_SPHERICAL: + return -1; + default: + return -1; + } + return 0; +} + +/** + * Get the acceleration direction unit vector for blend velocity calculations. + * This calculates the direction of acceleration at the end of a segment. + */ +int tcGetEndAccelUnitVector(TC_STRUCT const * const tc, PmCartesian * const out) { + + switch (tc->motion_type) { + case TC_LINEAR: + *out=tc->coords.line.xyz.uVec; + break; + case TC_RIGIDTAP: + pmCartScalMult(&tc->coords.line.xyz.uVec, -1.0, out); + break; + case TC_CIRCULAR: + tcCircleEndAccelUnitVector(tc,out); + break; + case TC_SPHERICAL: + return -1; + default: + return -1; + } + return 0; +} + +int tcGetIntersectionPoint(TC_STRUCT const * const prev_tc, + TC_STRUCT const * const tc, PmCartesian * const point) +{ + // TODO NULL pointer check? + // Get intersection point from geometry + if (tc->motion_type == TC_LINEAR) { + *point = tc->coords.line.xyz.start; + } else if (prev_tc->motion_type == TC_LINEAR) { + *point = prev_tc->coords.line.xyz.end; + } else if (tc->motion_type == TC_CIRCULAR){ + pmCirclePoint(&tc->coords.circle.xyz, 0.0, point); + } else { + return TP_ERR_FAIL; + } + return TP_ERR_OK; +} + + +/** + * Check if a segment can be consumed without disrupting motion or synced IO. + */ +int tcCanConsume(TC_STRUCT const * const tc) +{ + if (!tc) { + return false; + } + + if (tc->syncdio.anychanged || tc->blend_prev || tc->atspeed) { + //TODO add other conditions here (for any segment that should not be consumed by blending + return false; + } + + return true; + +} + +/** + * Find the geometric tangent vector to a helical arc. + * Unlike the acceleration vector, the result of this calculation is a vector + * tangent to the helical arc. This is called by wrapper functions for the case of a circular or helical arc. + */ +int pmCircleTangentVector(PmCircle const * const circle, + double angle_in, PmCartesian * const out) +{ + + PmCartesian startpoint; + PmCartesian radius; + PmCartesian uTan, dHelix, dRadial; + + // Get vector in radial direction + pmCirclePoint(circle, angle_in, &startpoint); + pmCartCartSub(&startpoint, &circle->center, &radius); + + /* Find local tangent vector using planar normal. Assuming a differential + * angle dtheta, the tangential component of the tangent vector is r * + * dtheta. Since we're normalizing the vector anyway, assume dtheta = 1. + */ + pmCartCartCross(&circle->normal, &radius, &uTan); + + /* the binormal component of the tangent vector is (dz / dtheta) * dtheta. + */ + double dz = 1.0 / circle->angle; + pmCartScalMult(&circle->rHelix, dz, &dHelix); + + pmCartCartAddEq(&uTan, &dHelix); + + /* The normal component is (dr / dtheta) * dtheta. + */ + double dr = circle->spiral / circle->angle; + pmCartUnit(&radius, &dRadial); + pmCartScalMultEq(&dRadial, dr); + pmCartCartAddEq(&uTan, &dRadial); + + //Normalize final output vector + pmCartUnit(&uTan, out); + return 0; +} + + +/** + * Calculate the unit tangent vector at the start of a move for any segment. + */ +int tcGetStartTangentUnitVector(TC_STRUCT const * const tc, PmCartesian * const out) { + + switch (tc->motion_type) { + case TC_LINEAR: + *out=tc->coords.line.xyz.uVec; + break; + case TC_RIGIDTAP: + *out=tc->coords.rigidtap.xyz.uVec; + break; + case TC_CIRCULAR: + pmCircleTangentVector(&tc->coords.circle.xyz, 0.0, out); + break; + default: + rtapi_print_msg(RTAPI_MSG_ERR, "Invalid motion type %d!\n",tc->motion_type); + return -1; + } + return 0; +} + +/** + * Calculate the unit tangent vector at the end of a move for any segment. + */ +int tcGetEndTangentUnitVector(TC_STRUCT const * const tc, PmCartesian * const out) { + + switch (tc->motion_type) { + case TC_LINEAR: + *out=tc->coords.line.xyz.uVec; + break; + case TC_RIGIDTAP: + pmCartScalMult(&tc->coords.rigidtap.xyz.uVec, -1.0, out); + break; + case TC_CIRCULAR: + pmCircleTangentVector(&tc->coords.circle.xyz, + tc->coords.circle.xyz.angle, out); + break; + default: + rtapi_print_msg(RTAPI_MSG_ERR, "Invalid motion type %d!\n",tc->motion_type); + return -1; + } + return 0; +} + +/** + * Calculate the unit tangent vector at the current progress position of a move. + * This gives the accurate direction at tc->progress, important for accurate jerk output. + */ +int tcGetCurrentTangentUnitVector(TC_STRUCT const * const tc, PmCartesian * const out) { + switch (tc->motion_type) { + case TC_LINEAR: + *out = tc->coords.line.xyz.uVec; + break; + case TC_RIGIDTAP: + if (tc->progress > 0.5 * tc->target) { + // Returning from tap, direction is reversed + pmCartScalMult(&tc->coords.rigidtap.xyz.uVec, -1.0, out); + } else { + *out = tc->coords.rigidtap.xyz.uVec; + } + break; + case TC_CIRCULAR: + { + // Calculate current angle based on progress + PmCircle const * const circle = &tc->coords.circle.xyz; + double angle = tc->progress / circle->radius; + // Handle spiral (radius varies) + if (circle->spiral != 0.0) { + // For spiral, use approximation + double r_ratio = circle->spiral * angle / (2.0 * PM_PI); + double avg_radius = circle->radius * (1.0 + r_ratio / 2.0); + angle = tc->progress / avg_radius; + } + pmCircleTangentVector(circle, angle, out); + } + break; + case TC_SPHERICAL: + { + // For spherical blend arcs, calculate current tangent + SphericalArc const * const arc = &tc->coords.arc.xyz; + + // Calculate progress fraction (0 to 1) + double total_length = arc->radius * arc->angle + arc->line_length; + double progress_frac = (total_length > DOUBLE_FUZZ) ? + tc->progress / total_length : 0.0; + + // Use endpoint tangent as approximation based on progress + // (arcTangent only supports at_end=0 or 1) + int at_end = (progress_frac > 0.5) ? 1 : 0; + arcTangent(arc, out, at_end); + } + break; + default: + rtapi_print_msg(RTAPI_MSG_ERR, "Invalid motion type %d in tcGetCurrentTangentUnitVector!\n", tc->motion_type); + return -1; + } + return 0; +} + +/** + * Calculate the distance left in the trajectory segment in the indicated + * direction. + */ +double tcGetDistanceToGo(TC_STRUCT const * const tc, int direction) +{ + double distance = tcGetTarget(tc, direction) - tc->progress; + if (direction == TC_DIR_REVERSE) { + distance *=-1.0; + } + return distance; +} + +double tcGetTarget(TC_STRUCT const * const tc, int direction) +{ + return (direction == TC_DIR_REVERSE) ? 0.0 : tc->target; +} + + +/*! tcGetPos() function + * + * \brief This function calculates the machine position along the motion's path. + * + * As we move along a TC, from zero to its length, we call this function repeatedly, + * with an increasing tc->progress. + * This function calculates the machine position along the motion's path + * corresponding to the current progress. + * It gets called at the end of tpRunCycle() + * + * @param tc the current TC that is being planned + * + * @return EmcPose returns a position (\ref EmcPose = datatype carrying XYZABC information + */ + +int tcGetPos(TC_STRUCT const * const tc, EmcPose * const out) { + tcGetPosReal(tc, TC_GET_PROGRESS, out); + return 0; +} + +int tcGetStartpoint(TC_STRUCT const * const tc, EmcPose * const out) { + tcGetPosReal(tc, TC_GET_STARTPOINT, out); + return 0; +} + +int tcGetEndpoint(TC_STRUCT const * const tc, EmcPose * const out) { + tcGetPosReal(tc, TC_GET_ENDPOINT, out); + return 0; +} + +int tcGetPosReal(TC_STRUCT const * const tc, int of_point, EmcPose * const pos) +{ + PmCartesian xyz; + PmCartesian abc; + PmCartesian uvw; + double progress=0.0; + + switch (of_point) { + case TC_GET_PROGRESS: + progress = tc->progress; + break; + case TC_GET_ENDPOINT: + progress = tc->target; + break; + case TC_GET_STARTPOINT: + progress = 0.0; + break; + } + + + // Used for arc-length to angle conversion with spiral segments + double angle = 0.0; + int res_fit = TP_ERR_OK; + + switch (tc->motion_type){ + case TC_RIGIDTAP: + if(tc->coords.rigidtap.state > REVERSING) { + pmCartLinePoint(&tc->coords.rigidtap.aux_xyz, progress, &xyz); + } else { + pmCartLinePoint(&tc->coords.rigidtap.xyz, progress, &xyz); + } + // no rotary move allowed while tapping + abc = tc->coords.rigidtap.abc; + uvw = tc->coords.rigidtap.uvw; + break; + case TC_LINEAR: + pmCartLinePoint(&tc->coords.line.xyz, + progress * tc->coords.line.xyz.tmag / tc->target, + &xyz); + pmCartLinePoint(&tc->coords.line.uvw, + progress * tc->coords.line.uvw.tmag / tc->target, + &uvw); + pmCartLinePoint(&tc->coords.line.abc, + progress * tc->coords.line.abc.tmag / tc->target, + &abc); + break; + case TC_CIRCULAR: + res_fit = pmCircleAngleFromProgress(&tc->coords.circle.xyz, + &tc->coords.circle.fit, + progress, &angle); + pmCirclePoint(&tc->coords.circle.xyz, + angle, + &xyz); + pmCartLinePoint(&tc->coords.circle.abc, + progress * tc->coords.circle.abc.tmag / tc->target, + &abc); + pmCartLinePoint(&tc->coords.circle.uvw, + progress * tc->coords.circle.uvw.tmag / tc->target, + &uvw); + break; + case TC_SPHERICAL: + arcPoint(&tc->coords.arc.xyz, + progress, + &xyz); + abc = tc->coords.arc.abc; + uvw = tc->coords.arc.uvw; + break; + } + + if (res_fit == TP_ERR_OK) { + // Don't touch pos unless we know the value is good + pmCartesianToEmcPose(&xyz, &abc, &uvw, pos); + } + return res_fit; +} + + +/** + * Set the terminal condition of a segment. + * This function will eventually handle state changes associated with altering a terminal condition. + */ +int tcSetTermCond(TC_STRUCT *prev_tc, TC_STRUCT *tc, int term_cond) { + switch (term_cond) { + case TC_TERM_COND_STOP: + case TC_TERM_COND_EXACT: + case TC_TERM_COND_TANGENT: + if (tc) {tc->blend_prev = 0;} + break; + case TC_TERM_COND_PARABOLIC: + if (tc) {tc->blend_prev = 1;} + break; + default: + break; + + } + if (prev_tc) { + tp_debug_print("setting term condition %d on tc id %d, type %d\n", term_cond, prev_tc->id, prev_tc->motion_type); + prev_tc->term_cond = term_cond; + } + return 0; +} + + +/** + * Connect a blend arc to the two line segments it blends. + * Starting with two adjacent line segments, this function shortens each + * segment to connect them with the newly created blend arc. The "previous" + * segment gets a new end point, while the next segment gets a new start point. + * After the operation is complete the result is a set of 3 connected segments + * (line-arc-line). + */ +int tcConnectBlendArc(TC_STRUCT * const prev_tc, TC_STRUCT * const tc, + PmCartesian const * const circ_start, + PmCartesian const * const circ_end) { + + /* Only shift XYZ for now*/ + if (prev_tc) { + tp_debug_print("connect: keep prev_tc\n"); + //Have prev line, need to shorten it + pmCartLineInit(&prev_tc->coords.line.xyz, + &prev_tc->coords.line.xyz.start, circ_start); + tp_debug_print("Old target = %f\n", prev_tc->target); + prev_tc->target = prev_tc->coords.line.xyz.tmag; + tp_debug_print("Target = %f\n",prev_tc->target); + //Setup tangent blending constraints + tcSetTermCond(prev_tc, tc, TC_TERM_COND_TANGENT); + tp_debug_print(" L1 end : %f %f %f\n",prev_tc->coords.line.xyz.end.x, + prev_tc->coords.line.xyz.end.y, + prev_tc->coords.line.xyz.end.z); + } else { + tp_debug_print("connect: consume prev_tc\n"); + } + + //Shorten next line + pmCartLineInit(&tc->coords.line.xyz, circ_end, &tc->coords.line.xyz.end); + + tp_info_print(" L2: old target = %f\n", tc->target); + tc->target = tc->coords.line.xyz.tmag; + tp_info_print(" L2: new target = %f\n", tc->target); + tp_debug_print(" L2 start : %f %f %f\n",tc->coords.line.xyz.start.x, + tc->coords.line.xyz.start.y, + tc->coords.line.xyz.start.z); + + tcSetTermCond(prev_tc, tc, TC_TERM_COND_TANGENT); + + tp_info_print(" Q1: %f %f %f\n",circ_start->x,circ_start->y,circ_start->z); + tp_info_print(" Q2: %f %f %f\n",circ_end->x,circ_end->y,circ_end->z); + + return 0; +} + + +/** + * Check if the current segment is actively blending. + * Checks if a blend should start based on acceleration and velocity criteria. + * Also saves this status so that the blend continues until the segment is + * done. + */ +int tcIsBlending(TC_STRUCT * const tc) { + //FIXME Disabling blends for rigid tap cycle until changes can be verified. + int is_blending_next = (tc->term_cond == TC_TERM_COND_PARABOLIC ) && + tc->on_final_decel && (tc->currentvel < tc->blend_vel) && + tc->motion_type != TC_RIGIDTAP; + + //Latch up the blending_next status here, so that even if the prev conditions + //aren't necessarily true we still blend to completion once the blend + //starts. + tc->blending_next |= is_blending_next; + + return tc->blending_next; +} + +int tcFindBlendTolerance(TC_STRUCT const * const prev_tc, + TC_STRUCT const * const tc, double * const T_blend, double * const nominal_tolerance) +{ + const double tolerance_ratio = 0.25; + double T1 = prev_tc->tolerance; + double T2 = tc->tolerance; + //Detect zero tolerance = no tolerance and force to reasonable maximum + if (T1 == 0) { + T1 = prev_tc->nominal_length * tolerance_ratio; + } + if (T2 == 0) { + T2 = tc->nominal_length * tolerance_ratio; + } + *nominal_tolerance = fmin(T1,T2); + //Blend tolerance is the limit of what we can reach by blending alone, + //consuming half a segment or less (parabolic equivalent) + double blend_tolerance = fmin(fmin(*nominal_tolerance, + prev_tc->nominal_length * tolerance_ratio), + tc->nominal_length * tolerance_ratio); + *T_blend = blend_tolerance; + return 0; +} + + +/** + * Check for early stop conditions. + * If a variety of conditions are true, then we can't do blending as we expect. + * This function checks for any conditions that force us to stop on the current + * segment. This is different from pausing or aborting, which can happen any + * time. + */ +int tcFlagEarlyStop(TC_STRUCT * const tc, + TC_STRUCT * const nexttc) +{ + + if (!tc || !nexttc) { + return TP_ERR_NO_ACTION; + } + + if(tc->synchronized != TC_SYNC_POSITION && nexttc->synchronized == TC_SYNC_POSITION) { + // we'll have to wait for spindle sync; might as well + // stop at the right place (don't blend) + tp_debug_print("waiting on spindle sync for tc %d\n", tc->id); + tcSetTermCond(tc, nexttc, TC_TERM_COND_STOP); + } + + if(nexttc->atspeed) { + // we'll have to wait for the spindle to be at-speed; might as well + // stop at the right place (don't blend), like above + // FIXME change the values so that 0 is exact stop mode + tp_debug_print("waiting on spindle atspeed for tc %d\n", tc->id); + tcSetTermCond(tc, nexttc, TC_TERM_COND_STOP); + } + + return TP_ERR_OK; +} + +double pmLine9Target(PmLine9 * const line9) +{ + if (!line9->xyz.tmag_zero) { + return line9->xyz.tmag; + } else if (!line9->uvw.tmag_zero) { + return line9->uvw.tmag; + } else if (!line9->abc.tmag_zero) { + return line9->abc.tmag; + } else { + return 0.0; + } +} + + +/** + * Initialize a new trajectory segment with common parameters. + * + * NOTE: this function only sets default values that are non-zero. Make sure + * the struct is properly initialized BEFORE calling this function. + */ +int tcInit(TC_STRUCT * const tc, + int motion_type, + int canon_motion_type, + double cycle_time, + unsigned char enables, + char atspeed) +{ + + /** Motion type setup */ + tc->motion_type = motion_type; + tc->canon_motion_type = canon_motion_type; + tc->atspeed = atspeed; + + /** Segment settings passed down from interpreter*/ + tc->enables = enables; + tc->cycle_time = cycle_time; + + tc->id = -1; //ID to be set when added to queue (may change before due to blend arcs) + + /** Segment settings (given values later during setup / optimization) */ + tc->indexer_jnum = -1; + + tc->active_depth = 1; + + tc->acc_ratio_tan = BLEND_ACC_RATIO_TANGENTIAL; + + // Initialize Ruckig planner fields + tc->ruckig_planner = NULL; + tc->ruckig_trajectory_time = 0.0; + tc->ruckig_planned = 0; + tc->ruckig_last_maxaccel = 0.0; + tc->ruckig_last_maxjerk = 0.0; + tc->ruckig_last_target_vel = 0.0; + tc->ruckig_last_final_vel = 0.0; + tc->ruckig_last_target_pos = 0.0; + tc->ruckig_last_use_velocity_control = 0; + tc->ruckig_last_req_pos = 0.0; + tc->ruckig_last_feed_override = 0.0; + + return TP_ERR_OK; +} + + +/** + * Set kinematic properties for a trajectory segment. + */ +int tcSetupMotion(TC_STRUCT * const tc, + double vel, + double ini_maxvel, + double acc, + double ini_maxjerk) +{ + //FIXME assumes that state is already set up in TC_STRUCT, which depends on external order of function calls. + + tc->maxaccel = acc; + tc->maxjerk = ini_maxjerk; + tc->blend_maxjerk = ini_maxjerk; // default equals maxjerk, look-ahead adjusts as needed + + tc->maxvel = ini_maxvel; + + tc->reqvel = vel; + // To be computed by velocity optimization / spindle-sync calculations + tc->target_vel = 0; + // To be filled in by tangent calculation, negative = invalid (KLUDGE) + tcInitKinkProperties(tc); + + return TP_ERR_OK; +} + + +int tcSetupState(TC_STRUCT * const tc, TP_STRUCT const * const tp) +{ + tcSetTermCond(tc, NULL, tp->termCond); + tc->tolerance = tp->tolerance; + tc->synchronized = tp->synchronized; + tc->uu_per_rev = tp->uu_per_rev; + return TP_ERR_OK; +} + +int pmLine9Init(PmLine9 * const line9, + EmcPose const * const start, + EmcPose const * const end) +{ + // Scratch variables + PmCartesian start_xyz, end_xyz; + PmCartesian start_uvw, end_uvw; + PmCartesian start_abc, end_abc; + + // Convert endpoint to cartesian representation + emcPoseToPmCartesian(start, &start_xyz, &start_abc, &start_uvw); + emcPoseToPmCartesian(end, &end_xyz, &end_abc, &end_uvw); + + // Initialize cartesian line members + int xyz_fail = pmCartLineInit(&line9->xyz, &start_xyz, &end_xyz); + int abc_fail = pmCartLineInit(&line9->abc, &start_abc, &end_abc); + int uvw_fail = pmCartLineInit(&line9->uvw, &start_uvw, &end_uvw); + + if (xyz_fail || abc_fail || uvw_fail) { + rtapi_print_msg(RTAPI_MSG_ERR,"Failed to initialize Line9, err codes %d, %d, %d\n", + xyz_fail,abc_fail,uvw_fail); + return TP_ERR_FAIL; + } + return TP_ERR_OK; +} + +int pmCircle9Init(PmCircle9 * const circ9, + EmcPose const * const start, + EmcPose const * const end, + PmCartesian const * const center, + PmCartesian const * const normal, + int turn) +{ + PmCartesian start_xyz, end_xyz; + PmCartesian start_uvw, end_uvw; + PmCartesian start_abc, end_abc; + + emcPoseToPmCartesian(start, &start_xyz, &start_abc, &start_uvw); + emcPoseToPmCartesian(end, &end_xyz, &end_abc, &end_uvw); + + int xyz_fail = pmCircleInit(&circ9->xyz, &start_xyz, &end_xyz, center, normal, turn); + //Initialize line parts of Circle9 + int abc_fail = pmCartLineInit(&circ9->abc, &start_abc, &end_abc); + int uvw_fail = pmCartLineInit(&circ9->uvw, &start_uvw, &end_uvw); + + int res_fit = findSpiralArcLengthFit(&circ9->xyz,&circ9->fit); + + if (xyz_fail || abc_fail || uvw_fail || res_fit) { + rtapi_print_msg(RTAPI_MSG_ERR,"Failed to initialize Circle9, err codes %d, %d, %d, %d\n", + xyz_fail, abc_fail, uvw_fail, res_fit); + return TP_ERR_FAIL; + } + return TP_ERR_OK; +} + +double pmCircle9Target(PmCircle9 const * const circ9) +{ + + double h2; + pmCartMagSq(&circ9->xyz.rHelix, &h2); + double helical_length = pmSqrt(pmSq(circ9->fit.total_planar_length) + h2); + + return helical_length; +} + +/** + * Apply acceleration and jerk limits to circular/spherical arc segments. + * + * For any arc (TC_CIRCULAR or TC_SPHERICAL), this function: + * 1. Limits velocity based on centripetal acceleration budget + * 2. For planner_type 1 (S-curve), applies three jerk constraints: + * - Steady-state rotational jerk: v³/R² + * - Normal jerk from tangential acceleration coupling: 3·v·a_t/R + * - Entry/exit transition jerk at arc boundaries + * 3. Calculates the tangential acceleration ratio for the arc + * + * This unified approach ensures consistent jerk limiting for both + * programmed arcs (G2/G3) and blend arcs at segment corners. + */ +int tcUpdateArcLimits(TC_STRUCT * tc) +{ + double radius, angle; + + // Extract radius and angle based on motion type + switch (tc->motion_type) { + case TC_CIRCULAR: + radius = pmCircleEffectiveMinRadius(&tc->coords.circle.xyz); + angle = tc->coords.circle.xyz.angle; + break; + case TC_SPHERICAL: + radius = tc->coords.arc.xyz.radius; + angle = tc->coords.arc.xyz.angle; + break; + default: + return 1; // Not an arc, nothing to do + } + + if (radius < DOUBLE_FUZZ || angle < TP_ANGLE_EPSILON) { + return 1; // Degenerate arc + } + + double a_max = tcGetOverallMaxAccel(tc); + double a_n_max_cutoff = BLEND_ACC_RATIO_NORMAL * a_max; + + // Find the acceleration necessary to reach the maximum velocity + double a_n_vmax = pmSq(tc->maxvel) / radius; + + // Find the maximum velocity that still obeys our desired normal/total acceleration ratio + double v_max_cutoff = pmSqrt(a_n_max_cutoff * radius); + + double v_max_actual = tc->maxvel; + double acc_ratio_tan = BLEND_ACC_RATIO_TANGENTIAL; + + if (a_n_vmax > a_n_max_cutoff) { + v_max_actual = v_max_cutoff; + } else { + acc_ratio_tan = pmSqrt(1.0 - pmSq(a_n_vmax / a_max)); + } + + // Jerk-based velocity limiting for S-curve planner (planner_type 1) + if (GET_TRAJ_PLANNER_TYPE() == 1 && emcmotStatus->jerk > TP_POS_EPSILON && + tc->cycle_time > TP_TIME_EPSILON) { + + double jerk = emcmotStatus->jerk; + double R_sq = pmSq(radius); + + // Constraint 1: Steady-state rotational jerk + entry/exit transitions + // The jerk budget is shared between steady-state (v³/R²) and transitions. + // Solving: v³ ≤ R² × j × φ / (2 + φ) + // The (2 + φ) term: 2 for two transitions, φ for steady-state budget + double v_max_jerk_steady = cruckig_cbrt(R_sq * jerk * angle / (2.0 + angle)); + + // Constraint 2: Normal jerk from tangential acceleration coupling + // During S-curve ramps on arc: j_n = 3·v·a_t/R + // Using BLEND_ACC_RATIO_TANGENTIAL as max tangential accel ratio + double a_t_max = BLEND_ACC_RATIO_TANGENTIAL * a_max; + double v_max_jerk_tan = jerk * radius / (3.0 * a_t_max); + + // Constraint 3: Entry/exit transition jerk (centripetal accel ramp) + // At line-arc boundary, centripetal accel changes from 0 to v²/R + // j_entry = (v²/R) / cycle_time ≤ j_max + double v_max_jerk_entry = pmSqrt(jerk * radius * tc->cycle_time); + + double v_max_jerk = fmin(fmin(v_max_jerk_steady, v_max_jerk_tan), v_max_jerk_entry); + + tp_debug_print("tcUpdateArcLimits: type=%d R=%f phi=%f j=%f\n", + tc->motion_type, radius, angle, jerk); + tp_debug_print(" v_jerk: steady=%f tan=%f entry=%f => min=%f\n", + v_max_jerk_steady, v_max_jerk_tan, v_max_jerk_entry, v_max_jerk); + + if (v_max_jerk < v_max_actual) { + tp_debug_print(" Limiting v_max from %f to %f for jerk\n", + v_max_actual, v_max_jerk); + v_max_actual = v_max_jerk; + + // Recalculate acc_ratio_tan for jerk-limited velocity + double a_n_at_jerk_vel = pmSq(v_max_actual) / radius; + if (a_n_at_jerk_vel < a_max) { + acc_ratio_tan = pmSqrt(1.0 - pmSq(a_n_at_jerk_vel / a_max)); + } + } + } + + tc->maxvel = v_max_actual; + tc->acc_ratio_tan = acc_ratio_tan; + + tp_debug_print("tcUpdateArcLimits: final v_max=%f acc_ratio_tan=%f\n", + tc->maxvel, tc->acc_ratio_tan); + + return 0; +} + +/** + * "Finalizes" a segment so that its length can't change. + * By setting the finalized flag, we tell the optimizer that this segment's + * length won't change anymore. Since any blends are already set up, we can + * trust that the length will be the same, and so can use the length in the + * velocity optimization. + */ +int tcFinalizeLength(TC_STRUCT * const tc) +{ + //Apply velocity corrections + if (!tc) { + return TP_ERR_FAIL; + } + + if (tc->finalized) { + tp_debug_print("tc %d already finalized\n", tc->id); + return TP_ERR_NO_ACTION; + } + + tp_debug_print("Finalizing motion id %d, type %d\n", tc->id, tc->motion_type); + + tcClampVelocityByLength(tc); + + tcUpdateArcLimits(tc); + + tc->finalized = 1; + return TP_ERR_OK; +} + + +int tcClampVelocityByLength(TC_STRUCT * const tc) +{ + //Apply velocity corrections + if (!tc) { + return TP_ERR_FAIL; + } + + //Reduce max velocity to match sample rate + //Assume that cycle time is valid here + double sample_maxvel = tc->target / tc->cycle_time; + tp_debug_print("sample_maxvel = %f\n",sample_maxvel); + tc->maxvel = fmin(tc->maxvel, sample_maxvel); + return TP_ERR_OK; +} + +/** + * compute the total arc length of a circle segment + */ +int tcUpdateTargetFromCircle(TC_STRUCT * const tc) +{ + if (!tc || tc->motion_type !=TC_CIRCULAR) { + return TP_ERR_FAIL; + } + + double h2; + pmCartMagSq(&tc->coords.circle.xyz.rHelix, &h2); + double helical_length = pmSqrt(pmSq(tc->coords.circle.fit.total_planar_length) + h2); + + tc->target = helical_length; + return TP_ERR_OK; +} + + + +int pmRigidTapInit(PmRigidTap * const tap, + EmcPose const * const start, + EmcPose const * const end, + double reversal_scale) +{ + PmCartesian start_xyz, end_xyz; + PmCartesian abc, uvw; + + //Slightly more allocation this way, but much easier to read + emcPoseToPmCartesian(start, &start_xyz, &abc, &uvw); + emcPoseGetXYZ(end, &end_xyz); + + // Setup XYZ motion + pmCartLineInit(&tap->xyz, &start_xyz, &end_xyz); + + // Copy over fixed ABC and UVW points + tap->abc = abc; + tap->uvw = uvw; + + // Setup initial tap state + tap->reversal_target = tap->xyz.tmag; + tap->reversal_scale = reversal_scale; + tap->state = RIGIDTAP_START; + return TP_ERR_OK; + +} + +double pmRigidTapTarget(PmRigidTap * const tap, double uu_per_rev) +{ + // allow 10 turns of the spindle to stop - we don't want to just go on forever + double overrun = 10. * uu_per_rev; + double target = tap->xyz.tmag + overrun; + tp_debug_print("initial tmag = %.12g, added %.12g for overrun, target = %.12g\n", + tap->xyz.tmag, overrun,target); + return target; +} + +/** Returns true if segment has ONLY rotary motion, false otherwise. */ +int tcPureRotaryCheck(TC_STRUCT const * const tc) +{ + return (tc->motion_type == TC_LINEAR) && + (tc->coords.line.xyz.tmag_zero) && + (tc->coords.line.uvw.tmag_zero); +} + + +/** + * Given a PmCircle and a circular segment, copy the circle in as the XYZ portion of the segment, then update the motion parameters. + * NOTE: does not yet support ABC or UVW motion! + */ +int tcSetCircleXYZ(TC_STRUCT * const tc, PmCircle const * const circ) +{ + + //Update targets with new arc length + if (!circ || tc->motion_type != TC_CIRCULAR) { + return TP_ERR_FAIL; + } + if (!tc->coords.circle.abc.tmag_zero || !tc->coords.circle.uvw.tmag_zero) { + rtapi_print_msg(RTAPI_MSG_ERR, "SetCircleXYZ does not supportABC or UVW motion\n"); + return TP_ERR_FAIL; + } + + // Store the new circular segment (or use the current one) + + if (!circ) { + rtapi_print_msg(RTAPI_MSG_ERR, "SetCircleXYZ missing new circle definition\n"); + return TP_ERR_FAIL; + } + + tc->coords.circle.xyz = *circ; + // Update the arc length fit to this new segment + findSpiralArcLengthFit(&tc->coords.circle.xyz, &tc->coords.circle.fit); + + // compute the new total arc length using the fit and store as new + // target distance + tc->target = pmCircle9Target(&tc->coords.circle); + + return TP_ERR_OK; +} + +/** + * Clean up Ruckig planner resources in a TC_STRUCT. + */ +void tcCleanupRuckig(TC_STRUCT * const tc) +{ + if (!tc) { + return; + } + + if (tc->ruckig_planner) { + ruckig_destroy(tc->ruckig_planner); + tc->ruckig_planner = NULL; + } + tc->ruckig_planned = 0; + tc->ruckig_trajectory_time = 0.0; +} + +int tcClearFlags(TC_STRUCT * const tc) +{ + if (!tc) { + return TP_ERR_MISSING_INPUT; + } + + //KLUDGE this will need to be updated manually if any other flags are added. + tc->is_blending = false; + + return TP_ERR_OK; +} + + diff --git a/wasm-port/vendor/linuxcnc/src/emc/tp/tc.h b/wasm-port/vendor/linuxcnc/src/emc/tp/tc.h new file mode 100644 index 0000000..d9d581b --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/tp/tc.h @@ -0,0 +1,128 @@ +/******************************************************************** +* Description: tc.h +* Discriminate-based trajectory planning +* +* Derived from a work by Fred Proctor & Will Shackleford +* +* Author: +* License: GPL Version 2 +* System: Linux +* +* Copyright (c) 2004 All rights reserved. +* +* Last change: +********************************************************************/ +#ifndef TC_H +#define TC_H + +#include +#include +#include + +#include "spherical_arc.h" +#include "tc_types.h" +#include "tp_types.h" + +double tcGetMaxTargetVel(TC_STRUCT const * const tc, + double max_scale); + +double tcGetOverallMaxAccel(TC_STRUCT const * tc); +double tcGetTangentialMaxAccel(TC_STRUCT const * const tc); + +int tcSetKinkProperties(TC_STRUCT *prev_tc, TC_STRUCT *tc, double kink_vel, double accel_reduction); +int tcInitKinkProperties(TC_STRUCT *tc); +int tcRemoveKinkProperties(TC_STRUCT *prev_tc, TC_STRUCT *tc); +int tcGetEndpoint(TC_STRUCT const * const tc, EmcPose * const out); +int tcGetStartpoint(TC_STRUCT const * const tc, EmcPose * const out); +int tcGetPos(TC_STRUCT const * const tc, EmcPose * const out); +int tcGetPosReal(TC_STRUCT const * const tc, int of_endpoint, EmcPose * const out); +int tcGetEndAccelUnitVector(TC_STRUCT const * const tc, PmCartesian * const out); +int tcGetStartAccelUnitVector(TC_STRUCT const * const tc, PmCartesian * const out); +int tcGetEndTangentUnitVector(TC_STRUCT const * const tc, PmCartesian * const out); +int tcGetStartTangentUnitVector(TC_STRUCT const * const tc, PmCartesian * const out); +int tcGetCurrentTangentUnitVector(TC_STRUCT const * const tc, PmCartesian * const out); + +double tcGetDistanceToGo(TC_STRUCT const * const tc, int direction); +double tcGetTarget(TC_STRUCT const * const tc, int direction); + +int tcGetIntersectionPoint(TC_STRUCT const * const prev_tc, + TC_STRUCT const * const tc, PmCartesian * const point); + +int tcCanConsume(TC_STRUCT const * const tc); + +int tcSetTermCond(TC_STRUCT * prev_tc, TC_STRUCT * tc, int term_cond); + +int tcConnectBlendArc(TC_STRUCT * const prev_tc, TC_STRUCT * const tc, + PmCartesian const * const circ_start, + PmCartesian const * const circ_end); + +int tcIsBlending(TC_STRUCT * const tc); + + +int tcFindBlendTolerance(TC_STRUCT const * const prev_tc, + TC_STRUCT const * const tc, double * const T_blend, double * const nominal_tolerance); + +int pmCircleTangentVector(PmCircle const * const circle, + double angle_in, PmCartesian * const out); + +int tcFlagEarlyStop(TC_STRUCT * const tc, + TC_STRUCT * const nexttc); + +double pmLine9Target(PmLine9 * const line9); + +int pmLine9Init(PmLine9 * const line9, + EmcPose const * const start, + EmcPose const * const end); + +double pmCircle9Target(PmCircle9 const * const circ9); + +int pmCircle9Init(PmCircle9 * const circ9, + EmcPose const * const start, + EmcPose const * const end, + PmCartesian const * const center, + PmCartesian const * const normal, + int turn); + +int pmRigidTapInit(PmRigidTap * const tap, + EmcPose const * const start, + EmcPose const * const end, + double reversal_scale); + +double pmRigidTapTarget(PmRigidTap * const tap, double uu_per_rev); + +int tcInit(TC_STRUCT * const tc, + int motion_type, + int canon_motion_type, + double cycle_time, + unsigned char enables, + char atspeed); + +int tcSetupFromTP(TC_STRUCT * const tc, TP_STRUCT const * const tp); + +int tcSetupMotion(TC_STRUCT * const tc, + double vel, + double ini_maxvel, + double acc, + double ini_maxjerk); + +int tcSetupState(TC_STRUCT * const tc, TP_STRUCT const * const tp); + +int tcUpdateArcLimits(TC_STRUCT * tc); + +int tcFinalizeLength(TC_STRUCT * const tc); + +int tcClampVelocityByLength(TC_STRUCT * const tc); + +int tcPureRotaryCheck(TC_STRUCT const * const tc); + +int tcSetCircleXYZ(TC_STRUCT * const tc, PmCircle const * const circ); + +int tcClearFlags(TC_STRUCT * const tc); + +/** + * Clean up Ruckig planner resources in a TC_STRUCT. + * Called when the trajectory segment is removed or reset. + */ +void tcCleanupRuckig(TC_STRUCT * const tc); + +#endif /* TC_H */ diff --git a/wasm-port/vendor/linuxcnc/src/emc/tp/tc_types.h b/wasm-port/vendor/linuxcnc/src/emc/tp/tc_types.h new file mode 100644 index 0000000..c1767d5 --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/tp/tc_types.h @@ -0,0 +1,215 @@ +/******************************************************************** +* Description: tc.h +* Discriminate-based trajectory planning +* +* Derived from a work by Fred Proctor & Will Shackleford +* +* Author: +* License: GPL Version 2 +* System: Linux +* +* Copyright (c) 2004 All rights reserved. +********************************************************************/ +#ifndef TC_TYPES_H +#define TC_TYPES_H + +#include +#include +#include + +#include "spherical_arc.h" +#include "../motion/state_tag.h" + +#define BLEND_DIST_FRACTION 0.5 +/* values for endFlag */ +typedef enum { + TC_TERM_COND_STOP = 0, + TC_TERM_COND_EXACT = 1, + TC_TERM_COND_PARABOLIC = 2, + TC_TERM_COND_TANGENT = 3 +} tc_term_cond_t; + +typedef enum { + TC_LINEAR = 1, + TC_CIRCULAR = 2, + TC_RIGIDTAP = 3, + TC_SPHERICAL = 4 +} tc_motion_type_t; + +typedef enum { + TC_SYNC_NONE = 0, + TC_SYNC_VELOCITY, + TC_SYNC_POSITION +} tc_spindle_sync_t; + +typedef enum { + TC_DIR_FORWARD = 0, + TC_DIR_REVERSE +} tc_direction_t; + +#define TC_GET_PROGRESS 0 +#define TC_GET_STARTPOINT 1 +#define TC_GET_ENDPOINT 2 + +#define TC_OPTIM_UNTOUCHED 0 +#define TC_OPTIM_AT_MAX 1 + +#define TC_ACCEL_TRAPZ 0 +#define TC_ACCEL_RAMP 1 + +/** + * Spiral arc length approximation by quadratic fit. + */ +typedef struct { + double b0; /* 2nd order coefficient */ + double b1; /* 1st order coefficient */ + double total_planar_length; /* total arc length in plane */ + int spiral_in; /* flag indicating spiral is inward, + rather than outward */ +} SpiralArcLengthFit; + + +/* structure for individual trajectory elements */ + +typedef struct { + PmCartLine xyz; + PmCartLine abc; + PmCartLine uvw; +} PmLine9; + +typedef struct { + PmCircle xyz; + PmCartLine abc; + PmCartLine uvw; + SpiralArcLengthFit fit; +} PmCircle9; + +typedef struct { + SphericalArc xyz; + PmCartesian abc; + PmCartesian uvw; +} Arc9; + +typedef enum { + RIGIDTAP_START, + TAPPING, REVERSING, RETRACTION, FINAL_REVERSAL, FINAL_PLACEMENT +} RIGIDTAP_STATE; + +typedef unsigned long long iomask_t; // 64 bits on both x86 and x86_64 + +typedef struct { + char anychanged; + iomask_t dio_mask; + iomask_t aio_mask; + signed char dios[EMCMOT_MAX_DIO]; + double aios[EMCMOT_MAX_AIO]; +} syncdio_t; + +typedef struct { + PmCartLine xyz; // original, but elongated, move down + PmCartLine aux_xyz; // this will be generated on the fly, for the other + // two moves: retraction, final placement + PmCartesian abc; + PmCartesian uvw; + double reversal_target; + double reversal_scale; + double spindlerevs_at_reversal; + RIGIDTAP_STATE state; +} PmRigidTap; + +typedef struct { + double cycle_time; + //Position stuff + double target; // actual segment length + double progress; // where are we in the segment? 0..target + double nominal_length; + + //Velocity + double reqvel; // vel requested by F word, calc'd by task + double target_vel; // velocity to actually track, limited by other factors + double maxvel; // max possible vel (feed override stops here) + double currentvel; // keep track of current step (vel * cycle_time) + double last_move_length;// last move length + double finalvel; // velocity to aim for at end of segment + double term_vel; // actual velocity at termination of segment + double kink_vel; // Temporary way to store our calculation of maximum velocity we can handle if this segment is declared tangent with the next + double kink_accel_reduce_prev; // How much to reduce the allowed tangential acceleration to account for the extra acceleration at an approximate tangent intersection. + double kink_accel_reduce; // How much to reduce the allowed tangential acceleration to account for the extra acceleration at an approximate tangent intersection. + + double factor; + + double targetvel; + double vt; + + //Jerk + double maxjerk; // max jerk for S-curve motion + double blend_maxjerk; // max jerk during blend (set by look-ahead) + double currentjerk; // current jerk for S-curve planning + double currentacc; // current acceleration for S-curve planning + double lastacc; + + //Acceleration + double maxaccel; // accel calc'd by task + double acc_ratio_tan;// ratio between normal and tangential accel + + int id; // segment's serial number + struct state_tag_t tag; // state tag corresponding to running motion + + union { // describes the segment's start and end positions + PmLine9 line; + PmCircle9 circle; + PmRigidTap rigidtap; + Arc9 arc; + } coords; + + int motion_type; // TC_LINEAR (coords.line) or + // TC_CIRCULAR (coords.circle) or + // TC_RIGIDTAP (coords.rigidtap) + int active; // this motion is being executed + int canon_motion_type; // this motion is due to which canon function? + int term_cond; // gcode requests continuous feed at the end of + // this segment (g64 mode) + + int blending_next; // segment is being blended into following segment + double blend_vel; // velocity below which we should start blending + double tolerance; // during the blend at the end of this move, + // stay within this distance from the path. + int synchronized; // spindle sync state + double uu_per_rev; // for sync, user units per rev (e.g. 0.0625 for 16tpi) + double vel_at_blend_start; + int sync_accel; // we're accelerating up to sync with the spindle + unsigned char enables; // Feed scale, etc, enable bits for this move + int atspeed; // wait for the spindle to be at-speed before starting this move + syncdio_t syncdio; // synched DIO's for this move. what to turn on/off + int indexer_jnum; // which joint to unlock (for a locking indexer) to make this move, -1 for none + int optimization_state; // At peak velocity during blends) + int on_final_decel; + int blend_prev; + int accel_mode; + int splitting; // the segment is less than 1 cycle time + // away from the end. + int remove; // Flag to remove the segment from the queue + int active_depth; /* Active depth (i.e. how many segments + * after this will it take to slow to zero + * speed) */ + int finalized; + + // Temporary status flags (reset each cycle) + int is_blending; + + // Ruckig trajectory planner support + void *ruckig_planner; // Ruckig planner handle (opaque pointer) + double ruckig_trajectory_time; // current trajectory time (seconds from trajectory start) + int ruckig_planned; // whether Ruckig planning completed (1=planned, 0=not) + // Store last planning parameters for detecting parameter changes + double ruckig_last_maxaccel; // max acceleration used in last planning + double ruckig_last_maxjerk; // max jerk used in last planning + double ruckig_last_target_vel; // target velocity used in last planning + double ruckig_last_final_vel; // final velocity used in last planning + double ruckig_last_target_pos; // target position used in last planning + int ruckig_last_use_velocity_control; // control mode used in last planning (1=velocity, 0=position) + double ruckig_last_req_pos; // last req_pos value from Ruckig (for velocity control incremental calc) + double ruckig_last_feed_override; // feed override value at last planning (for debug and change detection) +} TC_STRUCT; + +#endif /* TC_TYPES_H */ diff --git a/wasm-port/vendor/linuxcnc/src/emc/tp/tcq.c b/wasm-port/vendor/linuxcnc/src/emc/tp/tcq.c new file mode 100644 index 0000000..406cc2d --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/tp/tcq.c @@ -0,0 +1,355 @@ +/*! + ******************************************************************** + * Description: tcq.c + *\brief queue handling functions for trajectory planner + * These following functions implement the motion queue that + * is fed by tpAddLine/tpAddCircle and consumed by tpRunCycle. + * They have been fully working for a long time and a wise programmer + * won't mess with them. + * + *\author Derived from a work by Fred Proctor & Will Shackleford + *\author rewritten by Chris Radek + * + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2004 All rights reserved. + * + ********************************************************************/ + +#include + +#include "tcq.h" + +/** Return 0 if queue is valid, -1 if not */ +static inline int tcqCheck(TC_QUEUE_STRUCT const * const tcq) +{ + if ((0 == tcq) || (0 == tcq->queue)) + { + return -1; + } + return 0; +} + +/*! tcqCreate() function + * + * \brief Creates a new queue for TC elements. + * + * This function creates a new queue for TC elements. + * It gets called by tpCreate() + * + * @param tcq pointer to the new TC_QUEUE_STRUCT + * @param _size size of the new queue + * @param tcSpace holds the space allocated for the new queue, allocated in motion.c + * + * @return int returns success or failure + */ +int tcqCreate(TC_QUEUE_STRUCT * const tcq, int _size, TC_STRUCT * const tcSpace) +{ + if (!tcq || !tcSpace || _size < 1) { + return -1; + } + tcq->queue = tcSpace; + tcq->size = _size; + tcqInit(tcq); + + return 0; +} + +/*! tcqDelete() function + * + * \brief Deletes a queue holding TC elements. + * + * This function creates deletes a queue. It doesn't free the space + * only throws the pointer away. + * It gets called by tpDelete() + * \todo FIXME, it seems tpDelete() is gone, and this function isn't used. + * + * @param tcq pointer to the TC_QUEUE_STRUCT + * + * @return int returns success + */ +int tcqDelete(TC_QUEUE_STRUCT * const tcq) +{ + if (!tcqCheck(tcq)) { + /* free(tcq->queue); */ + tcq->queue = 0; + } + + return 0; +} + +/*! tcqInit() function + * + * \brief Initializes a queue with TC elements. + * + * This function initializes a queue with TC elements. + * It gets called by tpClear() and + * by tpRunCycle() when we are aborting + * + * @param tcq pointer to the TC_QUEUE_STRUCT + * + * @return int returns success or failure (if no tcq found) + */ +int tcqInit(TC_QUEUE_STRUCT * const tcq) +{ + if (tcqCheck(tcq)) return -1; + + tcq->_len = 0; + tcq->start = tcq->end = 0; + tcq->rend = 0; + tcq->_rlen = 0; + tcq->allFull = 0; + + return 0; +} + +/*! tcqPut() function + * + * \brief puts a TC element at the end of the queue + * + * This function adds a tc element at the end of the queue. + * It gets called by tpAddLine() and tpAddCircle() + * + * @param tcq pointer to the new TC_QUEUE_STRUCT + * @param tc the new TC element to be added + * + * @return int returns success or failure + */ +int tcqPut(TC_QUEUE_STRUCT * const tcq, TC_STRUCT const * const tc) +{ + /* check for initialized */ + if (tcqCheck(tcq)) return -1; + + /* check for allFull, so we don't overflow the queue */ + if (tcq->allFull) { + return -1; + } + + /* add it */ + tcq->queue[tcq->end] = *tc; + tcq->_len++; + + /* update end ptr, modulo size of queue */ + tcq->end = (tcq->end + 1) % tcq->size; + + /* set allFull flag if we're really full */ + if (tcq->end == tcq->start) { + tcq->allFull = 1; + } + + return 0; +} + + +/*! tcqPopBack() function + * + * \brief removes the newest TC element (converse of tcqRemove) + * + * @param tcq pointer to the TC_QUEUE_STRUCT + * + * @return int returns success or failure + */ +int tcqPopBack(TC_QUEUE_STRUCT * const tcq) +{ + /* check for initialized */ + if (tcqCheck(tcq)) return -1; + + /* Too short to pop! */ + if (tcq->_len < 1) { + return -1; + } + + int n = tcq->end - 1 + tcq->size; + tcq->end = n % tcq->size; + tcq->_len--; + + return 0; +} + +#define TCQ_REVERSE_MARGIN 200 + +int tcqPop(TC_QUEUE_STRUCT * const tcq) +{ + + if (tcqCheck(tcq)) { + return -1; + } + + if (tcq->_len < 1 && !tcq->allFull) { + return -1; + } + + /* update start ptr and reset allFull flag and len */ + tcq->start = (tcq->start + 1) % tcq->size; + tcq->allFull = 0; + tcq->_len--; + + if (tcq->_rlen < TCQ_REVERSE_MARGIN) { + //If we're not overwriting the history yet, then we have another segment added to the reverse history + tcq->_rlen++; + } else { + //If we're run out of spare reverse history, then advance rend + tcq->rend = (tcq->rend + 1) % tcq->size; + } + + return 0; +} + +/*! tcqRemove() function + * + * \brief removes n items from the queue + * + * This function removes the first n items from the queue, + * after checking that they can be removed + * (queue initialized, queue not empty, enough elements in it) + * Function gets called by tpRunCycle() with n=1 + * \todo FIXME: Optimize the code to remove only 1 element, might speed it up + * + * @param tcq pointer to the new TC_QUEUE_STRUCT + * @param n the number of TC elements to be removed + * + * @return int returns success or failure + */ +int tcqRemove(TC_QUEUE_STRUCT * const tcq, int n) +{ + + if (n <= 0) { + return 0; /* okay to remove 0 or fewer */ + } + + if (tcqCheck(tcq) || ((tcq->start == tcq->end) && !tcq->allFull) || + (n > tcq->_len)) { /* too many requested */ + return -1; + } + + /* update start ptr and reset allFull flag and len */ + tcq->start = (tcq->start + n) % tcq->size; + tcq->allFull = 0; + tcq->_len -= n; + + return 0; +} + + +/** + * Step backward into the reverse history. + */ +int tcqBackStep(TC_QUEUE_STRUCT * const tcq) +{ + + if (tcqCheck(tcq)) { + return -1; + } + + // start == end means that queue is empty + + if ( tcq->start == tcq->rend) { + return -1; + } + /* update start ptr and reset allFull flag and len */ + tcq->start = (tcq->start - 1 + tcq->size) % tcq->size; + tcq->_len++; + tcq->_rlen--; + + return 0; +} + +/*! tcqLen() function + * + * \brief returns the number of elements in the queue + * + * Function gets called by tpSetVScale(), tpAddLine(), tpAddCircle() + * + * @param tcq pointer to the TC_QUEUE_STRUCT + * + * @return int returns number of elements + */ +int tcqLen(TC_QUEUE_STRUCT const * const tcq) +{ + if (tcqCheck(tcq)) return -1; + + return tcq->_len; +} + +/*! tcqItem() function + * + * \brief gets the n-th TC element in the queue, without removing it + * + * Function gets called by tpSetVScale(), tpRunCycle(), tpIsPaused() + * + * @param tcq pointer to the TC_QUEUE_STRUCT + * + * @return TC_STRUCT returns the TC elements + */ +TC_STRUCT * tcqItem(TC_QUEUE_STRUCT const * const tcq, int n) +{ + if (tcqCheck(tcq) || (n < 0) || (n >= tcq->_len)) return NULL; + + return &(tcq->queue[(tcq->start + n) % tcq->size]); +} + +/*! + * \def TC_QUEUE_MARGIN + * sets up a margin at the end of the queue, to reduce effects of race conditions + */ +#define TC_QUEUE_MARGIN (TCQ_REVERSE_MARGIN+20) + +/*! tcqFull() function + * + * \brief get the full status of the queue + * Function returns full if the count is closer to the end of the queue than TC_QUEUE_MARGIN + * + * Function called by update_status() in control.c + * + * @param tcq pointer to the TC_QUEUE_STRUCT + * + * @return int returns status (0==not full, 1==full) + */ +int tcqFull(TC_QUEUE_STRUCT const * const tcq) +{ + if (tcqCheck(tcq)) { + return 1; /* null queue is full, for safety */ + } + + /* call the queue full if the length is into the margin, so reduce the + effect of a race condition where the appending process may not see the + full status immediately and send another motion */ + + if (tcq->size <= TC_QUEUE_MARGIN) { + /* no margin available, so full means really all full */ + return tcq->allFull; + } + + if (tcq->_len >= tcq->size - TC_QUEUE_MARGIN) { + /* we're into the margin, so call it full */ + return 1; + } + + /* we're not into the margin */ + return 0; +} + +/*! tcqLast() function + * + * \brief gets the last TC element in the queue, without removing it + * + * + * @param tcq pointer to the TC_QUEUE_STRUCT + * + * @return TC_STRUCT returns the TC element + */ +TC_STRUCT *tcqLast(TC_QUEUE_STRUCT const * const tcq) +{ + if (tcqCheck(tcq)) { + return NULL; + } + if (tcq->_len == 0) { + return NULL; + } + //Fix for negative modulus error + int n = tcq->end-1 + tcq->size; + return &(tcq->queue[n % tcq->size]); + +} + diff --git a/wasm-port/vendor/linuxcnc/src/emc/tp/tcq.h b/wasm-port/vendor/linuxcnc/src/emc/tp/tcq.h new file mode 100644 index 0000000..a7e137c --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/tp/tcq.h @@ -0,0 +1,75 @@ +/******************************************************************** + * Description: tcq.c + *\brief queue handling functions for trajectory planner + * These following functions implement the motion queue that + * is fed by tpAddLine/tpAddCircle and consumed by tpRunCycle. + * They have been fully working for a long time and a wise programmer + * won't mess with them. + * + * Derived from a work by Fred Proctor & Will Shackleford + * + * Author: + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2004 All rights reserved. + * + * Last change: + ********************************************************************/ + +/* queue of TC_STRUCT elements*/ +#ifndef TCQ_H +#define TCQ_H + +#include "tc_types.h" + +typedef struct { + TC_STRUCT *queue; /* ptr to the tcs */ + int size; /* size of queue */ + int _len; /* number of tcs now in queue */ + int _rlen; /* number of tcs now in reverse history */ + int start, end; /* indices to next to get, next to put */ + int rend; + int allFull; /* flag meaning it's actually full */ +} TC_QUEUE_STRUCT; + +/* TC_QUEUE_STRUCT functions */ + +/* create queue of _size */ +extern int tcqCreate(TC_QUEUE_STRUCT * const tcq, int _size, + TC_STRUCT * const tcSpace); + +/* free up queue */ +extern int tcqDelete(TC_QUEUE_STRUCT * const tcq); + +/* reset queue to empty */ +extern int tcqInit(TC_QUEUE_STRUCT * const tcq); + +/* put tc on end */ +extern int tcqPut(TC_QUEUE_STRUCT * const tcq, TC_STRUCT const * const tc); + +/* remove a single tc from the back of the queue */ +extern int tcqPopBack(TC_QUEUE_STRUCT * const tcq); + +extern int tcqPop(TC_QUEUE_STRUCT * const tcq); + +/* remove n tcs from front */ +extern int tcqRemove(TC_QUEUE_STRUCT * const tcq, int n); + +extern int tcqBackStep(TC_QUEUE_STRUCT * const tcq); + +/* how many tcs on queue */ +extern int tcqLen(TC_QUEUE_STRUCT const * const tcq); + +/* look at nth item, first is 0 */ +extern TC_STRUCT * tcqItem(TC_QUEUE_STRUCT const * const tcq, int n); + +/** + * Get the "end" of the queue, the most recently added item. + */ +extern TC_STRUCT * tcqLast(TC_QUEUE_STRUCT const * const tcq); + +/* get full status */ +extern int tcqFull(TC_QUEUE_STRUCT const * const tcq); + +#endif diff --git a/wasm-port/vendor/linuxcnc/src/emc/tp/tp.c b/wasm-port/vendor/linuxcnc/src/emc/tp/tp.c new file mode 100644 index 0000000..cd458ab --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/tp/tp.c @@ -0,0 +1,4388 @@ +/******************************************************************** +* Description: tp.c +* Trajectory planner based on TC elements +* +* Derived from a work by Fred Proctor & Will Shackleford +* +* Author: +* License: GPL Version 2 +* System: Linux +* +* Copyright (c) 2004 All rights reserved. +********************************************************************/ +#include /* rtapi_print_msg */ +#include +#include /* Geometry types & functions */ +#include +#include +#include "../motion/motion.h" +#include "../motion/mot_priv.h" +#include "../motion/axis.h" +#include "tp.h" +#include "tc.h" +#include "spherical_arc.h" +#include "blendmath.h" +//KLUDGE Don't include all of emc.hh here, just hand-copy the TERM COND +//definitions until we can break the emc constants out into a separate file. +//#include "emc.hh" +#define EMC_TRAJ_TERM_COND_STOP 0 +#define EMC_TRAJ_TERM_COND_EXACT 1 +#define EMC_TRAJ_TERM_COND_BLEND 2 + +/** + * @section tpdebugflags TP debugging flags + * Enable / disable various debugging functions here. + * These flags control debug printing from RTAPI. These functions are + * admittedly kludged on top of the existing rtapi_print framework. As written, + * though, it's an easy way to selectively compile functions as static or not, + * and selectively compile in assertions and debug printing. + */ + +#include "tp_debug.h" +#include "sp_scurve.h" +#include "ruckig_wrapper.h" +// FIXME: turn off this feature, which causes blends between rapids to +// use the feed override instead of the rapid override +#undef TP_SHOW_BLENDS + +#define TP_OPTIMIZATION_LAZY + +#define MAKE_TP_HAL_PINS +#undef MAKE_TP_HAL_PINS + +// api for tpCreate() inherits a component id provision to include hal pins: +// (not used by the this default tp implementation but may +// be used in alternate user-built implementations) +#ifdef MAKE_TP_HAL_PINS // { +#include +#endif // } + +emcmot_status_t *emcmotStatus; +emcmot_config_t *emcmotConfig; +emcmot_command_t *emcmotCommand; +emcmot_hal_data_t *emcmot_hal_data; + +#ifndef GET_TRAJ_PLANNER_TYPE +#define GET_TRAJ_PLANNER_TYPE() (emcmotStatus->planner_type) + +#define SET_TRAK_PLANNER_TYPE(tp) (emcmotStatus->planner_type = tp) + +#endif + +#define GET_TRAJ_HOME_USE_TP() (emcmotStatus->home_use_tp) + +//========================================================== +// tp module interface +// motmod function ptrs for functions called by tp: +static void( *_DioWrite)(int,char); +static void( *_AioWrite)(int,double); +static void( *_SetRotaryUnlock)(int,int); +static int ( *_GetRotaryIsUnlocked)(int); +static double(*_axis_get_vel_limit)(int); +static double(*_axis_get_acc_limit)(int); + +void tpMotFunctions(void( *pDioWrite)(int,char) + ,void( *pAioWrite)(int,double) + ,void( *pSetRotaryUnlock)(int,int) + ,int ( *pGetRotaryIsUnlocked)(int) + ,double(*paxis_get_vel_limit)(int) + ,double(*paxis_get_acc_limit)(int) + ) +{ + _DioWrite = pDioWrite; + _AioWrite = pAioWrite; + _SetRotaryUnlock = pSetRotaryUnlock; + _GetRotaryIsUnlocked = pGetRotaryIsUnlocked; + _axis_get_vel_limit = paxis_get_vel_limit; + _axis_get_acc_limit = paxis_get_acc_limit; +} + +void tpMotData(emcmot_status_t *pstatus + ,emcmot_config_t *pconfig + ) +{ + emcmotStatus = pstatus; + emcmotConfig = pconfig; +} +//========================================================= + +/** static function primitives (ugly but less of a pain than moving code around)*/ +STATIC int tpComputeBlendVelocity( + TC_STRUCT const *tc, + TC_STRUCT const *nexttc, + double v_target_this, + double v_target_next, + double *v_blend_this, + double *v_blend_next, + double *v_blend_net); + +STATIC int tpComputeBlendSCurveVelocity( + TC_STRUCT const *tc, + TC_STRUCT const *nexttc, + double target_vel_this, + double target_vel_next, + double *v_blend_this, + double *v_blend_next, + double *v_blend_net); + +STATIC double estimateParabolicBlendPerformance( + TP_STRUCT const *tp, + TC_STRUCT const *tc, + TC_STRUCT const *nexttc); + +STATIC int tpCheckEndCondition(TP_STRUCT const * const tp, TC_STRUCT * const tc, TC_STRUCT const * const nexttc); + +STATIC int tpUpdateCycle(TP_STRUCT * const tp, + TC_STRUCT * const tc, TC_STRUCT const * const nexttc, int* mode); + +STATIC int tpRunOptimization(TP_STRUCT * const tp); + +STATIC inline int tpAddSegmentToQueue(TP_STRUCT * const tp, TC_STRUCT * const tc, int inc_id); + +STATIC inline double tpGetMaxTargetVel(TP_STRUCT const * const tp, TC_STRUCT const * const tc); + +/** + * @section tpcheck Internal state check functions. + * These functions compartmentalize some of the messy state checks. + * Hopefully this makes changes easier to track as much of the churn will be on small functions. + */ + +/** + * Returns true if there is motion along ABC or UVW axes, false otherwise. + */ +STATIC int tcRotaryMotionCheck(TC_STRUCT const * const tc) { + switch (tc->motion_type) { + //Note lack of break statements due to every path returning + case TC_RIGIDTAP: + return false; + case TC_LINEAR: + if (tc->coords.line.abc.tmag_zero && tc->coords.line.uvw.tmag_zero) { + return false; + } else { + return true; + } + case TC_CIRCULAR: + if (tc->coords.circle.abc.tmag_zero && tc->coords.circle.uvw.tmag_zero) { + return false; + } else { + return true; + } + case TC_SPHERICAL: + return true; + default: + tp_debug_print("Unknown motion type!\n"); + return false; + } +} + + +/** + * @section tpgetset Internal Get/Set functions + * @brief Calculation / status functions for commonly used values. + * These functions return the "actual" values of things like a trajectory + * segment's feed override, while taking into account the status of tp itself. + */ + + + +/** + * Wrapper to bounds-check the tangent kink ratio from HAL. + */ +STATIC double tpGetTangentKinkRatio(void) { + const double max_ratio = 0.7071; + const double min_ratio = 0.001; + + return fmax(fmin(emcmotConfig->arcBlendTangentKinkRatio,max_ratio),min_ratio); +} + +STATIC int tpGetMachineAccelBounds(PmCartesian * const acc_bound) { + if (!acc_bound) { + return TP_ERR_FAIL; + } + + acc_bound->x = _axis_get_acc_limit(0); //0==>x + acc_bound->y = _axis_get_acc_limit(1); //1==>y + acc_bound->z = _axis_get_acc_limit(2); //2==>z + return TP_ERR_OK; +} + + +STATIC int tpGetMachineVelBounds(PmCartesian * const vel_bound) { + if (!vel_bound) { + return TP_ERR_FAIL; + } + + vel_bound->x = _axis_get_vel_limit(0); //0==>x + vel_bound->y = _axis_get_vel_limit(1); //1==>y + vel_bound->z = _axis_get_vel_limit(2); //2==>z + return TP_ERR_OK; +} + +STATIC int tpGetMachineActiveLimit(double * const act_limit, PmCartesian const * const bounds) { + if (!act_limit) { + return TP_ERR_FAIL; + } + //Start with max accel value + *act_limit = fmax(fmax(bounds->x,bounds->y),bounds->z); + + // Compare only with active axes + if (bounds->x > 0) { + *act_limit = fmin(*act_limit, bounds->x); + } + if (bounds->y > 0) { + *act_limit = fmin(*act_limit, bounds->y); + } + if (bounds->z > 0) { + *act_limit = fmin(*act_limit, bounds->z); + } + tp_debug_print(" arc blending a_max=%f\n", *act_limit); + return TP_ERR_OK; +} + + +/** + * Get a segment's feed scale based on the current planner state and emcmotStatus. + * @note depends on emcmotStatus for system information. + */ +STATIC double tpGetFeedScale(TP_STRUCT const * const tp, + TC_STRUCT const * const tc) { + if (!tc) { + return 0.0; + } + //All reasons to disable feed override go here + bool pausing = tp->pausing && (tc->synchronized == TC_SYNC_NONE || tc->synchronized == TC_SYNC_VELOCITY); + bool aborting = tp->aborting; + if (pausing) { + tc_debug_print("pausing\n"); + return 0.0; + } else if (aborting) { + tc_debug_print("aborting\n"); + return 0.0; + } else if (tc->synchronized == TC_SYNC_POSITION ) { + return 1.0; + } else if (tc->is_blending) { + //KLUDGE: Don't allow feed override to keep blending from overruning max velocity + return fmin(emcmotStatus->net_feed_scale, 1.0); + } else { + return emcmotStatus->net_feed_scale; + } +} + + +/** + * Get target velocity for a tc based on the trajectory planner state. + * This gives the requested velocity, capped by the segments maximum velocity. + */ +STATIC inline double tpGetRealTargetVel(TP_STRUCT const * const tp, + TC_STRUCT const * const tc) { + + if (!tc) { + return 0.0; + } + // Start with the scaled target velocity based on the current feed scale + double v_target = tc->synchronized ? tc->target_vel : tc->reqvel; + + // Get the maximum allowed target velocity, and make sure we're below it + return fmin(v_target * tpGetFeedScale(tp,tc), tpGetMaxTargetVel(tp, tc)); +} + + +STATIC inline double getMaxFeedScale(TC_STRUCT const * tc) +{ + //All reasons to disable feed override go here + if (tc && tc->synchronized == TC_SYNC_POSITION ) { + return 1.0; + } else { + return emcmotConfig->maxFeedScale; + } +} + + +/** + * Get the worst-case target velocity for a segment based on the trajectory planner state. + * Note that this factors in the user-specified velocity limit. + */ +STATIC inline double tpGetMaxTargetVel(TP_STRUCT const * const tp, TC_STRUCT const * const tc) +{ + double max_scale = emcmotConfig->maxFeedScale; + if (tc->is_blending) { + //KLUDGE: Don't allow feed override to keep blending from overruning max velocity + max_scale = fmin(max_scale, 1.0); + } + double v_max_target = tcGetMaxTargetVel(tc, max_scale); + + /* Check if the cartesian velocity limit applies and clip the maximum + * velocity. The vLimit is from the max velocity slider, and should + * restrict the maximum velocity during non-synced moves and velocity + * synchronization. However, position-synced moves have the target velocity + * computed in the TP, so it would disrupt position tracking to apply this + * limit here. + */ + if (!tcPureRotaryCheck(tc) && (tc->synchronized != TC_SYNC_POSITION)){ + /*tc_debug_print("Cartesian velocity limit active\n");*/ + v_max_target = fmin(v_max_target, tp->vLimit); + } + + return v_max_target; +} + + +/** + * Get final velocity for a tc based on the trajectory planner state. + * This function factors in the feed override and TC limits. It clamps the + * final velocity to the maximum velocity and the next segment's target velocity + */ +STATIC inline double tpGetRealFinalVel(TP_STRUCT const * const tp, + TC_STRUCT const * const tc, TC_STRUCT const * const nexttc) { + /* If we're stepping, then it doesn't matter what the optimization says, we want to end at a stop. + * If the term_cond gets changed out from under us, detect this and force final velocity to zero + */ + + if (emcmotStatus->stepping || tc->term_cond != TC_TERM_COND_TANGENT || tp->reverse_run) { + return 0.0; + } + + // Get target velocities for this segment and next segment + double v_target_this = tpGetRealTargetVel(tp, tc); + double v_target_next = 0.0; + if (nexttc) { + v_target_next = tpGetRealTargetVel(tp, nexttc); + } + + tc_debug_print("v_target_next = %f\n",v_target_next); + // Limit final velocity to minimum of this and next target velocities + double v_target = fmin(v_target_this, v_target_next); + return fmin(tc->finalvel, v_target); +} + +/** + * Convert the 2-part spindle position and sign to a signed double. + */ +STATIC inline double tpGetSignedSpindlePosition(spindle_status_t *status) { + int spindle_dir; + double spindle_pos; + spindle_dir = status->direction; + spindle_pos = status->spindleRevs; + if (spindle_dir < 0.0) { + spindle_pos*=-1.0; + } + return spindle_pos; +} + +/** + * @section tpaccess tp class-like API + */ + +/* space for trajectory planner queues, plus 10 more for safety */ +/*! \todo FIXME-- default is used; dynamic is not honored */ + TC_STRUCT queueTcSpace[DEFAULT_TC_QUEUE_SIZE + 10]; + +/** + * Create the trajectory planner structure with an empty queue. + */ + +#ifdef MAKE_TP_HAL_PINS // { +static struct tp_haldata { + // Example pin pointers + hal_u32_t *in; + hal_u32_t *out; + // Example parameters + hal_float_t param_rw; + hal_float_t param_ro; +} *tp_haldata; + +static int makepins(int id) { +#define HAL_PREFIX "tp" + int res=0; + if (id < 0) goto error; + tp_haldata = hal_malloc(sizeof(struct tp_haldata)); + if (!tp_haldata) goto error; + + // hal pin examples: + res += hal_pin_u32_newf(HAL_IN ,&(tp_haldata->in) ,id,"%s.in" ,HAL_PREFIX); + res += hal_pin_u32_newf(HAL_OUT,&(tp_haldata->out),id,"%s.out",HAL_PREFIX); + + // hal parameter examples: + res += hal_param_float_newf(HAL_RW, &tp_haldata->param_rw,id,"%s.param-rw",HAL_PREFIX); + res += hal_param_float_newf(HAL_RO, &tp_haldata->param_ro,id,"%s.param-ro",HAL_PREFIX); + + if (res) goto error; + rtapi_print("@@@ %s:%s: ok\n",__FILE__,__FUNCTION__); + return 0; // caller issues hal_ready() +error: + rtapi_print("\n!!! %s:%s: failed res=%d\n\n",__FILE__,__FUNCTION__,res); + return -1; +#undef HAL_PREFIX +} +#endif // } + +int tpCreate(TP_STRUCT * const tp, int _queueSize,int id) +{ + (void)id; + if (0 == tp) { + return TP_ERR_FAIL; + } + + if (_queueSize <= 0) { + tp->queueSize = TP_DEFAULT_QUEUE_SIZE; + } else { + tp->queueSize = _queueSize; + } + TC_STRUCT * const tcSpace = queueTcSpace; + + /* create the queue */ + if (-1 == tcqCreate(&tp->queue, tp->queueSize, tcSpace)) { + return TP_ERR_FAIL; + } + +#ifdef MAKE_TP_HAL_PINS // { + if (-1 == makepins(id)) { + return TP_ERR_FAIL; + } +#endif // } + + /* init the rest of our data */ + return tpInit(tp); +} + +/** + * Clears any potential DIO toggles and anychanged. + * If any DIOs need to be changed: dios[i] = 1, DIO needs to get turned on, -1 + * = off + */ +int tpClearDIOs(TP_STRUCT * const tp) { + //XXX: All IO's will be flushed on next synced aio/dio! Is it ok? + int i; + tp->syncdio.anychanged = 0; + tp->syncdio.dio_mask = 0; + tp->syncdio.aio_mask = 0; + for (i = 0; i < emcmotConfig->numDIO; i++) { + tp->syncdio.dios[i] = 0; + } + for (i = 0; i < emcmotConfig->numAIO; i++) { + tp->syncdio.aios[i] = 0; + } + + return TP_ERR_OK; +} + +/** + * "Soft initialize" the trajectory planner tp. + * This is a "soft" initialization in that TP_STRUCT configuration + * parameters (cycleTime, vMax, and aMax) are left alone, but the queue is + * cleared, and the flags are set to an empty, ready queue. The currentPos + * is left alone, and goalPos is set to this position. This function is + * intended to put the motion queue in the state it would be if all queued + * motions finished at the current position. + */ +int tpClear(TP_STRUCT * const tp) +{ + tcqInit(&tp->queue); + tp->queueSize = 0; + tp->goalPos = tp->currentPos; + // Clear out status ID's + tp->nextId = 0; + tp->execId = 0; + struct state_tag_t tag = {}; + tp->execTag = tag; + tp->motionType = 0; + tp->done = 1; + tp->depth = tp->activeDepth = 0; + tp->aborting = 0; + tp->pausing = 0; + tp->reverse_run = 0; + tp->synchronized = 0; + tp->uu_per_rev = 0.0; + emcmotStatus->current_vel = 0.0; + emcmotStatus->requested_vel = 0.0; + emcmotStatus->distance_to_go = 0.0; + ZERO_EMC_POSE(emcmotStatus->dtg); + + // equivalent to: SET_MOTION_INPOS_FLAG(1): + emcmotStatus->motionFlag |= EMCMOT_MOTION_INPOS_BIT; + + return tpClearDIOs(tp); +} + +/** + * Fully initialize the tp structure. + * Sets tp configuration to default values and calls tpClear to create a fresh, + * empty queue. + */ +int tpInit(TP_STRUCT * const tp) +{ + tp->cycleTime = 0.0; + //Velocity limits + tp->vLimit = 0.0; + tp->ini_maxvel = 0.0; + tp->ini_maxjerk = 0.0; + //Accelerations + tp->aLimit = 0.0; + PmCartesian acc_bound; + //FIXME this acceleration bound isn't valid (nor is it used) + if (emcmotStatus == 0) { + rtapi_print("!!!tpInit: NULL emcmotStatus, bye\n\n"); + return -1; + } + tpGetMachineAccelBounds(&acc_bound); + tpGetMachineActiveLimit(&tp->aMax, &acc_bound); + //Angular limits + tp->wMax = 0.0; + tp->wDotMax = 0.0; + + tp->spindle.offset = 0.0; + tp->spindle.revs = 0.0; + tp->spindle.waiting_for_index = MOTION_INVALID_ID; + tp->spindle.waiting_for_atspeed = MOTION_INVALID_ID; + + tp->reverse_run = TC_DIR_FORWARD; + tp->termCond = TC_TERM_COND_PARABOLIC; + tp->tolerance = 0.0; + + ZERO_EMC_POSE(tp->currentPos); + + PmCartesian vel_bound; + tpGetMachineVelBounds(&vel_bound); + tpGetMachineActiveLimit(&tp->vMax, &vel_bound); + + return tpClear(tp); +} + +/** + * Set the cycle time for the trajectory planner. + */ +int tpSetCycleTime(TP_STRUCT * const tp, double secs) +{ + if (0 == tp || secs <= 0.0) { + return TP_ERR_FAIL; + } + + tp->cycleTime = secs; + + /* initialize/update S-curve planner with cycle time */ + /* This is called in tpmod, where sp_scurve functions are used */ + if (sp_scurve_init(secs) != 0) { + rtapi_print_msg(RTAPI_MSG_ERR, "tpSetCycleTime: sp_scurve_init() failed with cycle_time=%f\n", secs); + // Not a fatal error, S-curve functionality may just be unavailable + } + + return TP_ERR_OK; +} + +/** + * Set requested velocity and absolute maximum velocity (bounded by machine). + * This is called before adding lines or circles, specifying vMax (the velocity + * requested by the F word) and ini_maxvel, the max velocity possible before + * meeting a machine constraint caused by an AXIS's max velocity. (the TP is + * allowed to go up to this high when feed override >100% is requested) These + * settings apply to subsequent moves until changed. + */ +int tpSetVmax(TP_STRUCT * const tp, double vMax, double ini_maxvel) +{ + if (0 == tp || vMax <= 0.0 || ini_maxvel <= 0.0) { + return TP_ERR_FAIL; + } + + tp->vMax = vMax; + tp->ini_maxvel = ini_maxvel; + + return TP_ERR_OK; +} + +/** + * Set the tool tip maximum velocity. + * This is the [TRAJ]MAX_LINEAR_VELOCITY. This should be the max velocity of + * const the TOOL TIP, not necessarily any particular axis. This applies to + * subsequent moves until changed. + */ +int tpSetVlimit(TP_STRUCT * const tp, double vLimit) +{ + if (!tp) return TP_ERR_FAIL; + + if (vLimit < 0.) + tp->vLimit = 0.; + else + tp->vLimit = vLimit; + + return TP_ERR_OK; +} + +/** Sets the max acceleration for the trajectory planner. */ +int tpSetAmax(TP_STRUCT * const tp, double aMax) +{ + if (0 == tp || aMax <= 0.0) { + return TP_ERR_FAIL; + } + + tp->aMax = aMax; + + return TP_ERR_OK; +} + +/** + * Sets the id that will be used for the next appended motions. + * nextId is incremented so that the next time a motion is appended its id will + * be one more than the previous one, modulo a signed int. If you want your own + * ids for each motion, call this before each motion you append and stick what + * you want in here. + */ +int tpSetId(TP_STRUCT * const tp, int id) +{ + + if (!MOTION_ID_VALID(id)) { + rtapi_print_msg(RTAPI_MSG_ERR, "tpSetId: invalid motion id %d\n", id); + return TP_ERR_FAIL; + } + + if (0 == tp) { + return TP_ERR_FAIL; + } + + tp->nextId = id; + + return TP_ERR_OK; +} + +/** Returns the id of the last motion that is currently + executing.*/ +int tpGetExecId(TP_STRUCT * const tp) +{ + if (0 == tp) { + return TP_ERR_FAIL; + } + + return tp->execId; +} + +struct state_tag_t tpGetExecTag(TP_STRUCT * const tp) +{ + if (0 == tp) { + struct state_tag_t empty = {}; + return empty; + } + + return tp->execTag; +} + + +/** + * Sets the termination condition for all subsequent queued moves. + * If cond is TC_TERM_COND_STOP, motion comes to a stop before a subsequent move + * begins. If cond is TC_TERM_COND_PARABOLIC, the following move is begun when the + * current move slows below a calculated blend velocity. + */ +int tpSetTermCond(TP_STRUCT * const tp, int cond, double tolerance) +{ + if (!tp) { + return TP_ERR_FAIL; + } + + switch (cond) { + //Purposeful waterfall for now + case TC_TERM_COND_PARABOLIC: + case TC_TERM_COND_TANGENT: + case TC_TERM_COND_EXACT: + case TC_TERM_COND_STOP: + tp->termCond = cond; + tp->tolerance = tolerance; + break; + default: + //Invalid condition + return -1; + } + + return TP_ERR_OK; +} + +/** + * Used to tell the tp the initial position. + * It sets the current position AND the goal position to be the same. Used + * only at TP initialization and when switching modes. + */ +int tpSetPos(TP_STRUCT * const tp, EmcPose const * const pos) +{ + if (0 == tp) { + return TP_ERR_FAIL; + } + + int res_invalid = tpSetCurrentPos(tp, pos); + if (res_invalid) { + return TP_ERR_FAIL; + } + + tp->goalPos = *pos; + return TP_ERR_OK; +} + + +/** + * Set current position. + * It sets the current position AND the goal position to be the same. Used + * only at TP initialization and when switching modes. + */ +int tpSetCurrentPos(TP_STRUCT * const tp, EmcPose const * const pos) +{ + if (0 == tp) { + return TP_ERR_FAIL; + } + + if (emcPoseValid(pos)) { + tp->currentPos = *pos; + return TP_ERR_OK; + } else { + rtapi_print_msg(RTAPI_MSG_ERR, "Tried to set invalid pose in tpSetCurrentPos on id %d!" + "pos is %.12g, %.12g, %.12g\n", + tp->execId, + pos->tran.x, + pos->tran.y, + pos->tran.z); + return TP_ERR_INVALID; + } +} + + +int tpAddCurrentPos(TP_STRUCT * const tp, EmcPose const * const disp) +{ + if (!tp || !disp) { + return TP_ERR_MISSING_INPUT; + } + + if (emcPoseValid(disp)) { + emcPoseSelfAdd(&tp->currentPos, disp); + return TP_ERR_OK; + } else { + rtapi_print_msg(RTAPI_MSG_ERR, "Tried to set invalid pose in tpAddCurrentPos on id %d!" + "disp is %.12g, %.12g, %.12g\n", + tp->execId, + disp->tran.x, + disp->tran.y, + disp->tran.z); + return TP_ERR_INVALID; + } +} + + +/** + * Check for valid tp before queueing additional moves. + */ +int tpErrorCheck(TP_STRUCT const * const tp) { + + if (!tp) { + rtapi_print_msg(RTAPI_MSG_ERR, "TP is null\n"); + return TP_ERR_FAIL; + } + if (tp->aborting) { + rtapi_print_msg(RTAPI_MSG_ERR, "TP is aborting\n"); + return TP_ERR_FAIL; + } + return TP_ERR_OK; +} + + +/** + * Find the "peak" velocity a segment can achieve if its velocity profile is triangular. + * This is used to estimate blend velocity, though by itself is not enough + * (since requested velocity and max velocity could be lower). + */ +STATIC double tpCalculateTriangleVel(TC_STRUCT const *tc) { + //Compute peak velocity for blend calculations + double acc_scaled = tcGetTangentialMaxAccel(tc); + double length = tc->target; + if (!tc->finalized) { + // blending may remove up to 1/2 of the segment + length /= 2.0; + } + return findVPeak(acc_scaled, length); +} + +/** + * Handles the special case of blending into an unfinalized segment. + * The problem here is that the last segment in the queue can always be cut + * short by a blend to the next segment. However, we can only ever consume at + * most 1/2 of the segment. This function computes the worst-case final + * velocity the previous segment can have, if we want to exactly stop at the + * halfway point. + */ +STATIC double tpCalculateOptimizationInitialVel(TP_STRUCT const * const tp, TC_STRUCT * const tc) +{ + double acc_scaled = tcGetTangentialMaxAccel(tc); + double triangle_vel = findVPeak(acc_scaled, tc->target); + double max_vel = tpGetMaxTargetVel(tp, tc); + tp_debug_json_start(tpCalculateOptimizationInitialVel); + tp_debug_json_double(triangle_vel); + tp_debug_json_end(); + return fmin(triangle_vel, max_vel); +} + +/** + * Find the "peak" velocity a segment can achieve if its velocity profile is triangular. + * This is used to estimate blend velocity, though by itself is not enough + * (since requested velocity and max velocity could be lower). + */ +STATIC double tpCalculateSCurveVel(TC_STRUCT const *tc) { + //Compute peak velocity for blend calculations + double acc_scaled = tcGetTangentialMaxAccel(tc); + double length = tc->target; + if (!tc->finalized) { + // blending may remove up to 1/2 of the segment + length /= 2.0; + } + return findSCurveVPeak(acc_scaled, emcmotStatus->jerk, length); +} + +/** + * Handles the special case of blending into an unfinalized segment. + * The problem here is that the last segment in the queue can always be cut + * short by a blend to the next segment. However, we can only ever consume at + * most 1/2 of the segment. This function computes the worst-case final + * velocity the previous segment can have, if we want to exactly stop at the + * halfway point. + */ +STATIC double tpCalculateOptimizationSCurveInitialVel(TP_STRUCT const * const tp, TC_STRUCT * const tc) +{ + double acc_scaled = tcGetTangentialMaxAccel(tc); + double maxjerk = fmin(tc->maxjerk, emcmotStatus->jerk); + double scurve_vel = findSCurveVPeak(acc_scaled, maxjerk, tc->target); + double max_vel = tpGetMaxTargetVel(tp, tc); + tp_debug_json_start(tpCalculateOptimizationSCurveInitialVel); + tp_debug_json_double(scurve_vel); + tp_debug_json_end(); + return fmin(scurve_vel, max_vel); +} + +/** + * Initialize a blend arc from its parent segments. + * This copies and initializes properties from the previous and next segments to + * initialize a blend arc. This function does not handle connecting the + * segments together, however. + */ +STATIC int tpInitBlendArcFromPrev(TP_STRUCT const * const tp, + TC_STRUCT const * const prev_tc, + TC_STRUCT* const blend_tc, + double vel, + double ini_maxvel, + double acc, + double ini_maxjerk) +{ + +#ifdef TP_SHOW_BLENDS + int canon_motion_type = EMC_MOTION_TYPE_ARC; +#else + int canon_motion_type = prev_tc->canon_motion_type; +#endif + + tcInit(blend_tc, + TC_SPHERICAL, + canon_motion_type, + tp->cycleTime, + prev_tc->enables, + false); // NOTE: blend arc never needs the atspeed flag, since the previous line will have it (and cannot be consumed). + + // Copy over state data from TP + tcSetupState(blend_tc, tp); + + // Set kinematics parameters from blend calculations + tcSetupMotion(blend_tc, + vel, + ini_maxvel, + acc, + ini_maxjerk); + + // Skip syncdio setup since this blend extends the previous line + blend_tc->syncdio = // enqueue the list of DIOs + prev_tc->syncdio; // that need toggling + + // find "helix" length for target + double length; + arcLength(&blend_tc->coords.arc.xyz, &length); + tp_info_print("blend tc length = %f\n",length); + blend_tc->target = length; + blend_tc->nominal_length = length; + + // Set the blend arc to be tangent to the next segment + tcSetTermCond(blend_tc, NULL, TC_TERM_COND_TANGENT); + + //NOTE: blend arc radius and everything else is finalized, so set this to 1. + //In the future, radius may be adjustable. + tcFinalizeLength(blend_tc); + + // copy state tag from previous segment during blend motion + blend_tc->tag = prev_tc->tag; + + return TP_ERR_OK; +} + +STATIC int tcSetLineXYZ(TC_STRUCT * const tc, PmCartLine const * const line) +{ + + //Update targets with new arc length + if (!line || tc->motion_type != TC_LINEAR) { + return TP_ERR_FAIL; + } + if (!tc->coords.line.abc.tmag_zero || !tc->coords.line.uvw.tmag_zero) { + rtapi_print_msg(RTAPI_MSG_ERR, "SetLineXYZ does not supportABC or UVW motion\n"); + return TP_ERR_FAIL; + } + + tc->coords.line.xyz = *line; + tc->target = line->tmag; + return TP_ERR_OK; +} + + +static inline int find_max_element(double arr[], int sz) +{ + if (sz < 1) { + return -1; + } + // Assumes at least one element + int max_idx = 0; + int idx; + for (idx = 0; idx < sz; ++idx) { + if (arr[idx] > arr[max_idx]) { + max_idx = idx; + } + } + return max_idx; +} + +/** + * Compare performance of blend arc and equivalent tangent speed. + * If we can go faster by assuming the segments are already tangent (and + * slowing down some), then prefer this over using the blend arc. This is + * mostly useful for some odd arc-to-arc cases where the blend arc becomes very + * short (and therefore slow). + */ +STATIC tc_blend_type_t tpChooseBestBlend(TP_STRUCT const * const tp, + TC_STRUCT * const prev_tc, + TC_STRUCT * const tc, + TC_STRUCT * const blend_tc) +{ + if (!tc || !prev_tc) { + return NO_BLEND; + } + + // Can't blend segments that are explicitly disallowed + switch (prev_tc->term_cond) + { + case TC_TERM_COND_EXACT: + case TC_TERM_COND_STOP: + return NO_BLEND; + } + + // Compute performance measures ("perf_xxx") for each method. This is + // basically the blend velocity. However, because parabolic blends require + // halving the acceleration of both blended segments, they in effect slow + // down the next and previous blends as well. We model this loss by scaling + // the blend velocity down to find an "equivalent" velocity. + double perf_parabolic = estimateParabolicBlendPerformance(tp, prev_tc, tc) / 2.0; + double perf_tangent = prev_tc->kink_vel; + double perf_arc_blend = blend_tc ? blend_tc->maxvel : 0.0; + + tp_debug_print("Blend performance: parabolic %f, tangent %f, arc_blend %f, ", + perf_parabolic, + perf_tangent, + perf_arc_blend); + + // KLUDGE Order the performance measurements so that they match the enum values + double perf[3] = {perf_parabolic, perf_tangent, perf_arc_blend}; + tc_blend_type_t best_blend = find_max_element(perf, 3); + + switch (best_blend) { + case PARABOLIC_BLEND: // parabolic + tp_debug_print("using parabolic blend\n"); + tcRemoveKinkProperties(prev_tc, tc); + tcSetTermCond(prev_tc, tc, TC_TERM_COND_PARABOLIC); + break; + case TANGENT_SEGMENTS_BLEND: // tangent + tp_debug_print("using approximate tangent blend\n"); + // NOTE: acceleration / velocity reduction is done dynamically in functions that access TC_STRUCT properties + tcSetTermCond(prev_tc, tc, TC_TERM_COND_TANGENT); + break; + case ARC_BLEND: // arc blend + tp_debug_print("using blend arc\n"); + tcRemoveKinkProperties(prev_tc, tc); + + break; + case NO_BLEND: + break; + } + return best_blend; +} + + +STATIC tp_err_t tpCreateLineArcBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc, TC_STRUCT * const tc, TC_STRUCT * const blend_tc) +{ + tp_debug_print("-- Starting LineArc blend arc --\n"); + + PmCartesian acc_bound, vel_bound; + + //Get machine limits + tpGetMachineAccelBounds(&acc_bound); + tpGetMachineVelBounds(&vel_bound); + + //Populate blend geometry struct + BlendGeom3 geom; + BlendParameters param; + BlendPoints3 points_approx; + BlendPoints3 points_exact; + + int res_init = blendInit3FromLineArc(&geom, ¶m, + prev_tc, + tc, + &acc_bound, + &vel_bound, + emcmotConfig->maxFeedScale); + + if (res_init != TP_ERR_OK) { + tp_debug_print("blend init failed with code %d, aborting blend arc\n", + res_init); + return res_init; + } + + // Check for coplanarity based on binormal and tangents + int coplanar = pmUnitCartsColinear(&geom.binormal, + &tc->coords.circle.xyz.normal); + + if (!coplanar) { + tp_debug_print("aborting arc, not coplanar\n"); + return TP_ERR_FAIL; + } + + int res_param = blendComputeParameters(¶m); + + int res_points = blendFindPoints3(&points_approx, &geom, ¶m); + + int res_post = blendLineArcPostProcess(&points_exact, + &points_approx, + ¶m, + &geom, &prev_tc->coords.line.xyz, + &tc->coords.circle.xyz); + + //Catch errors in blend setup + if (res_init || res_param || res_points || res_post) { + tp_debug_print("Got %d, %d, %d, %d for init, param, points, post, aborting arc\n", + res_init, + res_param, + res_points, + res_post); + return TP_ERR_FAIL; + } + + /* If blend calculations were successful, then we're ready to create the + * blend arc. + */ + + if (points_exact.trim2 > param.phi2_max) { + tp_debug_print("trim2 %f > phi2_max %f, aborting arc...\n", + points_exact.trim2, + param.phi2_max); + return TP_ERR_FAIL; + } + + blendCheckConsume(¶m, &points_exact, prev_tc, emcmotConfig->arcBlendGapCycles); + //Store working copies of geometry + PmCartLine line1_temp = prev_tc->coords.line.xyz; + PmCircle circ2_temp = tc->coords.circle.xyz; + + // Change lengths of circles + double new_len1 = line1_temp.tmag - points_exact.trim1; + int res_stretch1 = pmCartLineStretch(&line1_temp, + new_len1, + false); + + double phi2_new = tc->coords.circle.xyz.angle - points_exact.trim2; + + tp_debug_print("phi2_new = %f\n",phi2_new); + int res_stretch2 = pmCircleStretch(&circ2_temp, + phi2_new, + true); + //TODO create blends + if (res_stretch1 || res_stretch2) { + tp_debug_print("segment resize failed, aborting arc\n"); + return TP_ERR_FAIL; + } + + //Get exact start and end points to account for spiral in arcs + pmCartLinePoint(&line1_temp, + line1_temp.tmag, + &points_exact.arc_start); + pmCirclePoint(&circ2_temp, + 0.0, + &points_exact.arc_end); + //TODO deal with large spiral values, or else detect and fall back? + + blendPoints3Print(&points_exact); + int res_arc = arcFromBlendPoints3(&blend_tc->coords.arc.xyz, + &points_exact, + &geom, + ¶m); + if (res_arc < 0) { + tp_debug_print("arc creation failed, aborting arc\n"); + return TP_ERR_FAIL; + } + + // Note that previous restrictions don't allow ABC or UVW movement, so the + // end and start points should be identical + blend_tc->coords.arc.abc = prev_tc->coords.line.abc.end; + blend_tc->coords.arc.uvw = prev_tc->coords.line.uvw.end; + + //set the max velocity to v_plan, since we'll violate constraints otherwise. + tpInitBlendArcFromPrev(tp, prev_tc, blend_tc, param.v_req, + param.v_plan, param.a_max, fmin(tc->maxjerk, prev_tc->maxjerk)); + + int res_tangent = checkTangentAngle(&circ2_temp, + &blend_tc->coords.arc.xyz, + &geom, + ¶m, + tp->cycleTime, + true); + + if (res_tangent < 0) { + tp_debug_print("failed tangent check, aborting arc...\n"); + return TP_ERR_FAIL; + } + + if (tpChooseBestBlend(tp, prev_tc, tc, blend_tc) != ARC_BLEND) { + return TP_ERR_NO_ACTION; + } + + tp_debug_print("Passed all tests, updating segments\n"); + //TODO refactor to pass consume to connect function + if (param.consume) { + //Since we're consuming the previous segment, pop the last line off of the queue + int res_pop = tcqPopBack(&tp->queue); + if (res_pop) { + tp_debug_print("failed to pop segment, aborting arc\n"); + return TP_ERR_FAIL; + } + } else { + tcSetLineXYZ(prev_tc, &line1_temp); + //KLUDGE the previous segment is still there, so we don't need the at-speed flag on the blend too + blend_tc->atspeed=0; + } + tcSetCircleXYZ(tc, &circ2_temp); + + tcSetTermCond(prev_tc, tc, TC_TERM_COND_TANGENT); + + return TP_ERR_OK; +} + + +STATIC tp_err_t tpCreateArcLineBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc, TC_STRUCT * const tc, TC_STRUCT * const blend_tc) +{ + + tp_debug_print("-- Starting ArcLine blend arc --\n"); + PmCartesian acc_bound, vel_bound; + + //Get machine limits + tpGetMachineAccelBounds(&acc_bound); + tpGetMachineVelBounds(&vel_bound); + + //Populate blend geometry struct + BlendGeom3 geom; + BlendParameters param; + BlendPoints3 points_approx; + BlendPoints3 points_exact; + param.consume = 0; + + int res_init = blendInit3FromArcLine(&geom, ¶m, + prev_tc, + tc, + &acc_bound, + &vel_bound, + emcmotConfig->maxFeedScale); + if (res_init != TP_ERR_OK) { + tp_debug_print("blend init failed with code %d, aborting blend arc\n", + res_init); + return res_init; + } + + // Check for coplanarity based on binormal + int coplanar = pmUnitCartsColinear(&geom.binormal, + &prev_tc->coords.circle.xyz.normal); + + if (!coplanar) { + tp_debug_print("aborting arc, not coplanar\n"); + return TP_ERR_FAIL; + } + + int res_param = blendComputeParameters(¶m); + + int res_points = blendFindPoints3(&points_approx, &geom, ¶m); + + int res_post = blendArcLinePostProcess(&points_exact, + &points_approx, + ¶m, + &geom, &prev_tc->coords.circle.xyz, + &tc->coords.line.xyz); + + //Catch errors in blend setup + if (res_init || res_param || res_points || res_post) { + tp_debug_print("Got %d, %d, %d, %d for init, param, points, post\n", + res_init, + res_param, + res_points, + res_post); + return TP_ERR_FAIL; + } + + blendCheckConsume(¶m, &points_exact, prev_tc, emcmotConfig->arcBlendGapCycles); + + /* If blend calculations were successful, then we're ready to create the + * blend arc. + */ + + // Store working copies of geometry + PmCircle circ1_temp = prev_tc->coords.circle.xyz; + PmCartLine line2_temp = tc->coords.line.xyz; + + // Update start and end points of segment copies + double phi1_new = circ1_temp.angle - points_exact.trim1; + + if (points_exact.trim1 > param.phi1_max) { + tp_debug_print("trim1 %f > phi1_max %f, aborting arc...\n", + points_exact.trim1, + param.phi1_max); + return TP_ERR_FAIL; + } + + int res_stretch1 = pmCircleStretch(&circ1_temp, + phi1_new, + false); + if (res_stretch1 != TP_ERR_OK) { + return TP_ERR_FAIL; + } + + double new_len2 = tc->target - points_exact.trim2; + int res_stretch2 = pmCartLineStretch(&line2_temp, + new_len2, + true); + + if (res_stretch1 || res_stretch2) { + tp_debug_print("segment resize failed, aborting arc\n"); + return TP_ERR_FAIL; + } + + pmCirclePoint(&circ1_temp, + circ1_temp.angle, + &points_exact.arc_start); + + pmCartLinePoint(&line2_temp, + 0.0, + &points_exact.arc_end); + + blendPoints3Print(&points_exact); + + int res_arc = arcFromBlendPoints3(&blend_tc->coords.arc.xyz, &points_exact, &geom, ¶m); + if (res_arc < 0) { + return TP_ERR_FAIL; + } + + // Note that previous restrictions don't allow ABC or UVW movement, so the + // end and start points should be identical + blend_tc->coords.arc.abc = tc->coords.line.abc.start; + blend_tc->coords.arc.uvw = tc->coords.line.uvw.start; + + //set the max velocity to v_plan, since we'll violate constraints otherwise. + tpInitBlendArcFromPrev(tp, prev_tc, blend_tc, param.v_req, + param.v_plan, param.a_max, fmin(tc->maxjerk, prev_tc->maxjerk)); + + int res_tangent = checkTangentAngle(&circ1_temp, &blend_tc->coords.arc.xyz, &geom, ¶m, tp->cycleTime, false); + if (res_tangent) { + tp_debug_print("failed tangent check, aborting arc...\n"); + return TP_ERR_FAIL; + } + + if (tpChooseBestBlend(tp, prev_tc, tc, blend_tc) != ARC_BLEND) { + return TP_ERR_NO_ACTION; + } + + tp_debug_print("Passed all tests, updating segments\n"); + + tcSetCircleXYZ(prev_tc, &circ1_temp); + tcSetLineXYZ(tc, &line2_temp); + + //Cleanup any mess from parabolic + tc->blend_prev = 0; + blend_tc->atspeed=0; + tcSetTermCond(prev_tc, tc, TC_TERM_COND_TANGENT); + return TP_ERR_OK; +} + +STATIC tp_err_t tpCreateArcArcBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc, TC_STRUCT * const tc, TC_STRUCT * const blend_tc) +{ + + tp_debug_print("-- Starting ArcArc blend arc --\n"); + //TODO type checks + int colinear = pmUnitCartsColinear(&prev_tc->coords.circle.xyz.normal, + &tc->coords.circle.xyz.normal); + if (!colinear) { + // Fail out if not collinear + tp_debug_print("arc abort: not coplanar\n"); + return TP_ERR_FAIL; + } + + PmCartesian acc_bound, vel_bound; + + //Get machine limits + tpGetMachineAccelBounds(&acc_bound); + tpGetMachineVelBounds(&vel_bound); + + //Populate blend geometry struct + BlendGeom3 geom; + BlendParameters param; + BlendPoints3 points_approx; + BlendPoints3 points_exact; + + int res_init = blendInit3FromArcArc(&geom, ¶m, + prev_tc, + tc, + &acc_bound, + &vel_bound, + emcmotConfig->maxFeedScale); + + if (res_init != TP_ERR_OK) { + tp_debug_print("blend init failed with code %d, aborting blend arc\n", + res_init); + return res_init; + } + + int coplanar1 = pmUnitCartsColinear(&geom.binormal, + &prev_tc->coords.circle.xyz.normal); + + if (!coplanar1) { + tp_debug_print("aborting blend arc, arc id %d is not coplanar with binormal\n", prev_tc->id); + return TP_ERR_FAIL; + } + + int coplanar2 = pmUnitCartsColinear(&geom.binormal, + &tc->coords.circle.xyz.normal); + if (!coplanar2) { + tp_debug_print("aborting blend arc, arc id %d is not coplanar with binormal\n", tc->id); + return TP_ERR_FAIL; + } + + + + int res_param = blendComputeParameters(¶m); + int res_points = blendFindPoints3(&points_approx, &geom, ¶m); + + int res_post = blendArcArcPostProcess(&points_exact, + &points_approx, + ¶m, + &geom, &prev_tc->coords.circle.xyz, + &tc->coords.circle.xyz); + + //Catch errors in blend setup + if (res_init || res_param || res_points || res_post) { + tp_debug_print("Got %d, %d, %d, %d for init, param, points, post\n", + res_init, + res_param, + res_points, + res_post); + + return TP_ERR_FAIL; + } + + blendCheckConsume(¶m, &points_exact, prev_tc, emcmotConfig->arcBlendGapCycles); + + /* If blend calculations were successful, then we're ready to create the + * blend arc. Begin work on temp copies of each circle here: + */ + + double phi1_new = prev_tc->coords.circle.xyz.angle - points_exact.trim1; + double phi2_new = tc->coords.circle.xyz.angle - points_exact.trim2; + + // TODO pare down this debug output + tp_debug_print("phi1_new = %f, trim1 = %f\n", phi1_new, points_exact.trim1); + tp_debug_print("phi2_new = %f, trim2 = %f\n", phi2_new, points_exact.trim2); + + if (points_exact.trim1 > param.phi1_max) { + tp_debug_print("trim1 %f > phi1_max %f, aborting arc...\n", + points_exact.trim1, + param.phi1_max); + return TP_ERR_FAIL; + } + + if (points_exact.trim2 > param.phi2_max) { + tp_debug_print("trim2 %f > phi2_max %f, aborting arc...\n", + points_exact.trim2, + param.phi2_max); + return TP_ERR_FAIL; + } + + //Store working copies of geometry + PmCircle circ1_temp = prev_tc->coords.circle.xyz; + PmCircle circ2_temp = tc->coords.circle.xyz; + + int res_stretch1 = pmCircleStretch(&circ1_temp, + phi1_new, + false); + if (res_stretch1 != TP_ERR_OK) { + return TP_ERR_FAIL; + } + + int res_stretch2 = pmCircleStretch(&circ2_temp, + phi2_new, + true); + if (res_stretch1 || res_stretch2) { + tp_debug_print("segment resize failed, aborting arc\n"); + return TP_ERR_FAIL; + } + + //Get exact start and end points to account for spiral in arcs + pmCirclePoint(&circ1_temp, + circ1_temp.angle, + &points_exact.arc_start); + pmCirclePoint(&circ2_temp, + 0.0, + &points_exact.arc_end); + + tp_debug_print("Modified arc points\n"); + blendPoints3Print(&points_exact); + int res_arc = arcFromBlendPoints3(&blend_tc->coords.arc.xyz, &points_exact, &geom, ¶m); + if (res_arc < 0) { + return TP_ERR_FAIL; + } + + // Note that previous restrictions don't allow ABC or UVW movement, so the + // end and start points should be identical + blend_tc->coords.arc.abc = prev_tc->coords.circle.abc.end; + blend_tc->coords.arc.uvw = prev_tc->coords.circle.uvw.end; + + //set the max velocity to v_plan, since we'll violate constraints otherwise. + tpInitBlendArcFromPrev(tp, prev_tc, blend_tc, param.v_req, + param.v_plan, param.a_max, fmin(tc->maxjerk, prev_tc->maxjerk)); + + int res_tangent1 = checkTangentAngle(&circ1_temp, &blend_tc->coords.arc.xyz, &geom, ¶m, tp->cycleTime, false); + int res_tangent2 = checkTangentAngle(&circ2_temp, &blend_tc->coords.arc.xyz, &geom, ¶m, tp->cycleTime, true); + if (res_tangent1 || res_tangent2) { + tp_debug_print("failed tangent check, aborting arc...\n"); + return TP_ERR_FAIL; + } + + if (tpChooseBestBlend(tp, prev_tc, tc, blend_tc) != ARC_BLEND) { + return TP_ERR_NO_ACTION; + } + + tp_debug_print("Passed all tests, updating segments\n"); + + tcSetCircleXYZ(prev_tc, &circ1_temp); + tcSetCircleXYZ(tc, &circ2_temp); + + //Cleanup any mess from parabolic + tc->blend_prev = 0; + blend_tc->atspeed=0; + tcSetTermCond(prev_tc, tc, TC_TERM_COND_TANGENT); + return TP_ERR_OK; +} + + +STATIC tp_err_t tpCreateLineLineBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc, + TC_STRUCT * const tc, TC_STRUCT * const blend_tc) +{ + + tp_debug_print("-- Starting LineLine blend arc --\n"); + PmCartesian acc_bound, vel_bound; + + //Get machine limits + tpGetMachineAccelBounds(&acc_bound); + tpGetMachineVelBounds(&vel_bound); + + // Setup blend data structures + BlendGeom3 geom; + BlendParameters param; + BlendPoints3 points; + + int res_init = blendInit3FromLineLine(&geom, ¶m, + prev_tc, + tc, + &acc_bound, + &vel_bound, + emcmotConfig->maxFeedScale); + + if (res_init != TP_ERR_OK) { + tp_debug_print("blend init failed with code %d, aborting blend arc\n", + res_init); + return res_init; + } + + int res_blend = blendComputeParameters(¶m); + if (res_blend != TP_ERR_OK) { + return res_blend; + } + + blendFindPoints3(&points, &geom, ¶m); + + blendCheckConsume(¶m, &points, prev_tc, emcmotConfig->arcBlendGapCycles); + + // Set up actual blend arc here + int res_arc = arcFromBlendPoints3(&blend_tc->coords.arc.xyz, &points, &geom, ¶m); + if (res_arc < 0) { + return TP_ERR_FAIL; + } + + // Note that previous restrictions don't allow ABC or UVW movement, so the + // end and start points should be identical + blend_tc->coords.arc.abc = prev_tc->coords.line.abc.end; + blend_tc->coords.arc.uvw = prev_tc->coords.line.uvw.end; + + //set the max velocity to v_plan, since we'll violate constraints otherwise. + tpInitBlendArcFromPrev(tp, prev_tc, blend_tc, param.v_req, + param.v_plan, param.a_max, fmin(tc->maxjerk, prev_tc->maxjerk)); + + tp_debug_print("blend_tc target_vel = %g\n", blend_tc->target_vel); + + if (tpChooseBestBlend(tp, prev_tc, tc, blend_tc) != ARC_BLEND) { + return TP_ERR_NO_ACTION; + } + + int retval = TP_ERR_FAIL; + + //TODO refactor to pass consume to connect function + if (param.consume) { + //Since we're consuming the previous segment, pop the last line off of the queue + retval = tcqPopBack(&tp->queue); + if (retval) { + //This is unrecoverable since we've already changed the line. Something is wrong if we get here... + rtapi_print_msg(RTAPI_MSG_ERR, "PopBack failed\n"); + return TP_ERR_FAIL; + } + //Since the blend arc meets the end of the previous line, we only need + //to "connect" to the next line + retval = tcConnectBlendArc(NULL, tc, &points.arc_start, &points.arc_end); + } else { + //TODO refactor connect function to stretch lines and check for bad stretching + tp_debug_print("keeping previous line\n"); + retval = tcConnectBlendArc(prev_tc, tc, &points.arc_start, &points.arc_end); + blend_tc->atspeed=0; + } + return retval; +} + + +/** + * Add a newly created motion segment to the tp queue. + * Returns an error code if the queue operation fails, otherwise adds a new + * segment to the queue and updates the end point of the trajectory planner. + */ +STATIC inline int tpAddSegmentToQueue(TP_STRUCT * const tp, TC_STRUCT * const tc, int inc_id) { + + tc->id = tp->nextId; + if (tcqPut(&tp->queue, tc) == -1) { + rtapi_print_msg(RTAPI_MSG_ERR, "tcqPut failed.\n"); + return TP_ERR_FAIL; + } + if (inc_id) { + tp->nextId++; + } + + // Store end of current move as new final goal of TP + // KLUDGE: endpoint is garbage for rigid tap since it's supposed to retract past the start point. + if (tc->motion_type != TC_RIGIDTAP) { + tcGetEndpoint(tc, &tp->goalPos); + } + tp->done = 0; + tp->depth = tcqLen(&tp->queue); + //Fixing issue with duplicate id's? + tp_debug_print("Adding TC id %d of type %d, total length %0.08f\n",tc->id,tc->motion_type,tc->target); + + return TP_ERR_OK; +} + +STATIC int handleModeChange(TC_STRUCT * const prev_tc, TC_STRUCT * const tc) +{ + if (!tc || !prev_tc) { + return TP_ERR_FAIL; + } + if ((prev_tc->canon_motion_type == EMC_MOTION_TYPE_TRAVERSE) ^ + (tc->canon_motion_type == EMC_MOTION_TYPE_TRAVERSE)) { + tp_debug_print("Blending disabled: can't blend between rapid and feed motions\n"); + tcSetTermCond(prev_tc, tc, TC_TERM_COND_STOP); + } + if (prev_tc->synchronized != TC_SYNC_POSITION && + tc->synchronized == TC_SYNC_POSITION) { + tp_debug_print("Blending disabled: changing spindle sync mode from %d to %d\n", + prev_tc->synchronized, + tc->synchronized); + tcSetTermCond(prev_tc, tc, TC_TERM_COND_STOP); + } + return TP_ERR_OK; +} + +STATIC int tpSetupSyncedIO(TP_STRUCT * const tp, TC_STRUCT * const tc) { + if (tp->syncdio.anychanged != 0) { + tc->syncdio = tp->syncdio; //enqueue the list of DIOs that need toggling + tpClearDIOs(tp); // clear out the list, in order to prepare for the next time we need to use it + return TP_ERR_OK; + } else { + tc->syncdio.anychanged = 0; + return TP_ERR_NO_ACTION; + } + + +} + + +/** + * Adds a rigid tap cycle to the motion queue. + */ +int tpAddRigidTap(TP_STRUCT * const tp, + EmcPose end, + double vel, + double ini_maxvel, + double acc, + double ini_maxjerk, + unsigned char enables, + double scale, + struct state_tag_t tag) { + + if (tpErrorCheck(tp)) { + return TP_ERR_FAIL; + } + + tp_info_print("== AddRigidTap ==\n"); + + if(!tp->synchronized) { + rtapi_print_msg(RTAPI_MSG_ERR, "Cannot add unsynchronized rigid tap move.\n"); + return TP_ERR_FAIL; + } + + TC_STRUCT tc = {0}; + + /* Initialize rigid tap move. + * NOTE: rigid tapping does not have a canonical type. + * NOTE: always need atspeed since this is a synchronized movement. + * */ + tcInit(&tc, + TC_RIGIDTAP, + 0, + tp->cycleTime, + enables, + 1); + tc.tag = tag; + + // Setup any synced IO for this move + tpSetupSyncedIO(tp, &tc); + + // Copy over state data from the trajectory planner + tcSetupState(&tc, tp); + + // Copy in motion parameters + tcSetupMotion(&tc, + vel, + ini_maxvel, + acc, + ini_maxjerk); + + // Setup rigid tap geometry + pmRigidTapInit(&tc.coords.rigidtap, + &tp->goalPos, + &end, scale); + tc.target = pmRigidTapTarget(&tc.coords.rigidtap, tp->uu_per_rev); + + // Force exact stop mode after rigid tapping regardless of TP setting + tcSetTermCond(&tc, NULL, TC_TERM_COND_STOP); + + TC_STRUCT *prev_tc; + //Assume non-zero error code is failure + prev_tc = tcqLast(&tp->queue); + tcFinalizeLength(prev_tc); + tcFlagEarlyStop(prev_tc, &tc); + int retval = tpAddSegmentToQueue(tp, &tc, true); + tpRunOptimization(tp); + return retval; +} + +STATIC blend_type_t tpCheckBlendArcType( + TC_STRUCT const * const prev_tc, + TC_STRUCT const * const tc) { + + if (!prev_tc || !tc) { + tp_debug_print("prev_tc or tc doesn't exist\n"); + return BLEND_NONE; + } + + //If exact stop, we don't compute the arc + if (prev_tc->term_cond != TC_TERM_COND_PARABOLIC) { + // term_cond is signed int; use %d (cppcheck invalidPrintfArgType_uint) + tp_debug_print("Wrong term cond = %d\n", prev_tc->term_cond); + return BLEND_NONE; + } + + //If we have any rotary axis motion, then don't create a blend arc + if (tcRotaryMotionCheck(tc) || tcRotaryMotionCheck(prev_tc)) { + tp_debug_print("One of the segments has rotary motion, aborting blend arc\n"); + return BLEND_NONE; + } + + if (tc->finalized || prev_tc->finalized) { + tp_debug_print("Can't create blend when segment lengths are finalized\n"); + return BLEND_NONE; + } + + // motion_type is signed int; use %d (cppcheck invalidPrintfArgType_uint) + tp_debug_print("Motion types: prev_tc = %d, tc = %d\n", + prev_tc->motion_type,tc->motion_type); + //If not linear blends, we can't easily compute an arc + if ((prev_tc->motion_type == TC_LINEAR) && (tc->motion_type == TC_LINEAR)) { + return BLEND_LINE_LINE; + } else if (prev_tc->motion_type == TC_LINEAR && tc->motion_type == TC_CIRCULAR) { + return BLEND_LINE_ARC; + } else if (prev_tc->motion_type == TC_CIRCULAR && tc->motion_type == TC_LINEAR) { + return BLEND_ARC_LINE; + } else if (prev_tc->motion_type == TC_CIRCULAR && tc->motion_type == TC_CIRCULAR) { + return BLEND_ARC_ARC; + } else { + return BLEND_NONE; + } +} + + +/** + * Based on the nth and (n-1)th segment, find a safe final velocity for the (n-1)th segment. + * This function also caps the target velocity if velocity ramping is enabled. If we + * don't do this, then the linear segments (with higher tangential + * acceleration) will speed up and slow down to reach their target velocity, + * creating "humps" in the velocity profile. + */ +STATIC int tpComputeOptimalVelocity(TP_STRUCT const * const tp, TC_STRUCT * const tc, TC_STRUCT * const prev1_tc) { + //Calculate the maximum starting velocity vs_back of segment tc, given the + //trajectory parameters + double acc_this = tcGetTangentialMaxAccel(tc); + + // Find the reachable velocity of tc, moving backwards in time + // Calculate max start speed that can decelerate to tc->finalvel within tc->target + double vs_back; + if(GET_TRAJ_PLANNER_TYPE() == 1){ + // S-curve mode: use findSCurveMaxStartSpeed for reverse velocity optimization + // Use minimum of segment's max jerk and system max jerk to ensure limits are not exceeded + double maxjerk = fmin(tc->maxjerk, emcmotStatus->jerk); + double vs_back2 = pmSqrt(pmSq(tc->finalvel) + 2.0 * acc_this * tc->target); + if(findSCurveMaxStartSpeed(tc->target, tc->finalvel, acc_this, maxjerk, &vs_back) != 1){ + // S-curve calculation failed, use conservative estimate (at least maintain finalvel) + vs_back = tc->finalvel; + } + vs_back = fmin(vs_back, vs_back2); + } else { + // Trapezoidal mode: v^2 = v0^2 + 2as + vs_back = pmSqrt(pmSq(tc->finalvel) + 2.0 * acc_this * tc->target); + } + // Find the reachable velocity of prev1_tc, moving forwards in time + + double vf_limit_this = tc->maxvel; + double vf_limit_prev = prev1_tc->maxvel; + if (prev1_tc->kink_vel >=0 && prev1_tc->term_cond == TC_TERM_COND_TANGENT) { + // Only care about kink_vel with tangent segments + vf_limit_prev = fmin(vf_limit_prev, prev1_tc->kink_vel); + } + //Limit the PREVIOUS velocity by how much we can overshoot into + double vf_limit = fmin(vf_limit_this, vf_limit_prev); + + if (vs_back >= vf_limit ) { + //If we've hit the requested velocity, then prev_tc is definitely a "peak" + vs_back = vf_limit; + prev1_tc->optimization_state = TC_OPTIM_AT_MAX; + tp_debug_print("found peak due to v_limit %f\n", vf_limit); + } + + /* S-curve: prev1_tc's finalvel must not exceed the peak velocity reachable + * within prev1_tc's length under jerk constraints, otherwise Ruckig cannot + * plan from current velocity to that finalvel within prev1_tc->target */ + if (GET_TRAJ_PLANNER_TYPE() == 1) { + double acc_prev = tcGetTangentialMaxAccel(prev1_tc); + double jerk_prev = fmin(prev1_tc->maxjerk, emcmotStatus->jerk); + double prev_max_end_vel = findSCurveVPeak(acc_prev, jerk_prev, prev1_tc->target); + vs_back = fmin(vs_back, prev_max_end_vel); + } + + //Limit tc's target velocity to avoid creating "humps" in the velocity profile + prev1_tc->finalvel = vs_back; + + //Reduce max velocity to match sample rate + double sample_maxvel = tc->target / (tp->cycleTime * TP_MIN_SEGMENT_CYCLES); + tc->maxvel = fmin(tc->maxvel, sample_maxvel); + + tp_info_print(" prev1_tc-> fv = %f, tc->fv = %f\n", + prev1_tc->finalvel, tc->finalvel); + + return TP_ERR_OK; +} + + +/** + * Do "rising tide" optimization to find allowable final velocities for each queued segment. + * Walk along the queue from the back to the front. Based on the "current" + * segment's final velocity, calculate the previous segment's maximum allowable + * final velocity. The depth we walk along the queue is controlled by the + * TP_LOOKAHEAD_DEPTH constant for now. The process safely aborts early due to + * a short queue or other conflicts. + */ +STATIC int tpRunOptimization(TP_STRUCT * const tp) { + // Pointers to the "current", previous, and 2nd previous trajectory + // components. Current in this context means the segment being optimized, + // NOT the currently executing segment. + + TC_STRUCT *tc; + TC_STRUCT *prev1_tc; + + int ind, x; + int len = tcqLen(&tp->queue); + //TODO make lookahead depth configurable from the INI file + + int hit_peaks = 0; + // Flag that says we've hit at least 1 non-tangent segment + bool hit_non_tangent = false; + + /* Starting at the 2nd to last element in the queue, work backwards towards + * the front. We can't do anything with the very last element because its + * length may change if a new line is added to the queue.*/ + + for (x = 1; x < emcmotConfig->arcBlendOptDepth + 2; ++x) { + tp_info_print("==== Optimization step %d ====\n",x); + + // Update the pointers to the trajectory segments in use + ind = len-x; + tc = tcqItem(&tp->queue, ind); + prev1_tc = tcqItem(&tp->queue, ind-1); + + if ( !prev1_tc || !tc) { + tp_debug_print(" Reached end of queue in optimization\n"); + return TP_ERR_OK; + } + + // stop optimizing if we hit a non-tangent segment (final velocity + // stays zero) + if (prev1_tc->term_cond != TC_TERM_COND_TANGENT) { + if (hit_non_tangent) { + // 2 or more non-tangent segments means we're past where the optimizer can help + tp_debug_print("Found 2nd non-tangent segment, stopping optimization\n"); + return TP_ERR_OK; + } else { + tp_debug_print("Found first non-tangent segment, continuing\n"); + hit_non_tangent = true; + continue; + } + } + + double progress_ratio = prev1_tc->progress / prev1_tc->target; + // can safely decelerate to halfway point of segment from 25% of segment + double cutoff_ratio = BLEND_DIST_FRACTION / 2.0; + + if (progress_ratio >= cutoff_ratio) { + tp_debug_print("segment %d has moved past %f percent progress, cannot blend safely!\n", + ind-1, cutoff_ratio * 100.0); + return TP_ERR_OK; + } + + //Somewhat pedantic check for other conditions that would make blending unsafe + if (prev1_tc->splitting || prev1_tc->blending_next) { + tp_debug_print("segment %d is already blending, cannot optimize safely!\n", + ind-1); + return TP_ERR_OK; + } + + tp_info_print(" current term = %u, type = %u, id = %u, accel_mode = %d\n", + tc->term_cond, tc->motion_type, tc->id, tc->accel_mode); + tp_info_print(" prev term = %u, type = %u, id = %u, accel_mode = %d\n", + prev1_tc->term_cond, prev1_tc->motion_type, prev1_tc->id, prev1_tc->accel_mode); + + if (tc->atspeed) { + //Assume worst case that we have a stop at this point. This may cause a + //slight hiccup, but the alternative is a sudden hard stop. + tp_debug_print("Found atspeed at id %d\n",tc->id); + tc->finalvel = 0.0; + } + + if (!tc->finalized) { + tp_debug_print("Segment %d, type %d not finalized, continuing\n",tc->id,tc->motion_type); + // use worst-case final velocity that allows for up to 1/2 of a segment to be consumed. + + if(GET_TRAJ_PLANNER_TYPE() == 1) + prev1_tc->finalvel = fmin(prev1_tc->maxvel, tpCalculateOptimizationSCurveInitialVel(tp,tc)); + else + prev1_tc->finalvel = fmin(prev1_tc->maxvel, tpCalculateOptimizationInitialVel(tp,tc)); + + // Fixes acceleration violations when last segment is not finalized, and previous segment is tangent. + if (prev1_tc->kink_vel >=0 && prev1_tc->term_cond == TC_TERM_COND_TANGENT) { + prev1_tc->finalvel = fmin(prev1_tc->finalvel, prev1_tc->kink_vel); + } + tc->finalvel = 0.0; + } else { + tpComputeOptimalVelocity(tp, tc, prev1_tc); + } + + tc->active_depth = x - 2 - hit_peaks; +#ifdef TP_OPTIMIZATION_LAZY + if (tc->optimization_state == TC_OPTIM_AT_MAX) { + hit_peaks++; + } + if (hit_peaks > TP_OPTIMIZATION_CUTOFF) { + return TP_ERR_OK; + } +#endif + + } + tp_debug_print("Reached optimization depth limit\n"); + return TP_ERR_OK; +} + + +/** + * Check for tangency between the current segment and previous segment. + * If the current and previous segment are tangent, then flag the previous + * segment as tangent, and limit the current segment's velocity by the sampling + * rate. + */ +STATIC int tpSetupTangent(TP_STRUCT const * const tp, + TC_STRUCT * const prev_tc, TC_STRUCT * const tc) { + if (!tc || !prev_tc) { + tp_debug_print("missing tc or prev tc in tangent check\n"); + return TP_ERR_FAIL; + } + //If we have ABCUVW movement, then don't check for tangency + if (tcRotaryMotionCheck(tc) || tcRotaryMotionCheck(prev_tc)) { + tp_debug_print("found rotary axis motion\n"); + return TP_ERR_FAIL; + } + + if (emcmotConfig->arcBlendOptDepth < 2) { + tp_debug_print("Optimization depth %d too low for tangent optimization\n", + emcmotConfig->arcBlendOptDepth); + return TP_ERR_FAIL; + } + + if (prev_tc->term_cond == TC_TERM_COND_STOP) { + tp_debug_print("Found exact stop condition\n"); + return TP_ERR_FAIL; + } + + PmCartesian prev_tan, this_tan; + + int res_endtan = tcGetEndTangentUnitVector(prev_tc, &prev_tan); + int res_starttan = tcGetStartTangentUnitVector(tc, &this_tan); + if (res_endtan || res_starttan) { + tp_debug_print("Got %d and %d from tangent vector calc\n", + res_endtan, res_starttan); + } + + tp_debug_print("prev tangent vector: %f %f %f\n", prev_tan.x, prev_tan.y, prev_tan.z); + tp_debug_print("this tangent vector: %f %f %f\n", this_tan.x, this_tan.y, this_tan.z); + + // Assume small angle approximation here + const double SHARP_CORNER_DEG = 2.0; + const double SHARP_CORNER_EPSILON = pmSq(PM_PI * ( SHARP_CORNER_DEG / 180.0)); + if (pmCartCartAntiParallel(&prev_tan, &this_tan, SHARP_CORNER_EPSILON)) + { + tp_debug_print("Found sharp corner\n"); + tcSetTermCond(prev_tc, tc, TC_TERM_COND_STOP); + return TP_ERR_FAIL; + } + + // Calculate instantaneous acceleration required for change in direction + // from v1 to v2, assuming constant speed + double v_max1 = tcGetMaxTargetVel(prev_tc, getMaxFeedScale(prev_tc)); + double v_max2 = tcGetMaxTargetVel(tc, getMaxFeedScale(tc)); + // Note that this is a minimum since the velocity at the intersection must + // be the slower of the two segments not to violate constraints. + double v_max = fmin(v_max1, v_max2); + tp_debug_print("tangent v_max = %f\n",v_max); + + // Account for acceleration past final velocity during a split cycle + // (e.g. next segment starts accelerating again so the average velocity is higher at the end of the split cycle) + double a_inst = v_max / tp->cycleTime + tc->maxaccel; + // Set up worst-case final velocity + // Compute the actual magnitude of acceleration required given the tangent directions + // Do this by assuming that we decelerate to a stop on the previous segment, + // and simultaneously accelerate up to the maximum speed on the next one. + PmCartesian acc1, acc2, acc_diff; + pmCartScalMult(&prev_tan, a_inst, &acc1); + pmCartScalMult(&this_tan, a_inst, &acc2); + pmCartCartSub(&acc2,&acc1,&acc_diff); + + //TODO store this in TP struct instead? + PmCartesian acc_bound; + tpGetMachineAccelBounds(&acc_bound); + + PmCartesian acc_scale; + findAccelScale(&acc_diff,&acc_bound,&acc_scale); + tp_debug_print("acc_diff: %f %f %f\n", + acc_diff.x, + acc_diff.y, + acc_diff.z); + tp_debug_print("acc_scale: %f %f %f\n", + acc_scale.x, + acc_scale.y, + acc_scale.z); + + //FIXME this ratio is arbitrary, should be more easily tunable + double acc_scale_max = pmCartAbsMax(&acc_scale); + //KLUDGE lumping a few calculations together here + if (prev_tc->motion_type == TC_CIRCULAR || tc->motion_type == TC_CIRCULAR) { + acc_scale_max /= BLEND_ACC_RATIO_TANGENTIAL; + } + + // Controls the tradeoff between reduction of final velocity, and reduction of allowed segment acceleration + // TODO: this should ideally depend on some function of segment length and acceleration for better optimization + const double kink_ratio = tpGetTangentKinkRatio(); + + if (acc_scale_max < kink_ratio) { + tp_debug_print(" Kink acceleration within %g, using tangent blend\n", kink_ratio); + tcSetTermCond(prev_tc, tc, TC_TERM_COND_TANGENT); + tcSetKinkProperties(prev_tc, tc, v_max, acc_scale_max); + return TP_ERR_OK; + } else { + tcSetKinkProperties(prev_tc, tc, v_max * kink_ratio / acc_scale_max, kink_ratio); + tp_debug_print("Kink acceleration scale %f above %f, kink vel = %f, blend arc may be faster\n", + acc_scale_max, + kink_ratio, + prev_tc->kink_vel); + // NOTE: acceleration will be reduced later if tangent blend is used + return TP_ERR_NO_ACTION; + } +} + +static bool tpCreateBlendIfPossible( + TP_STRUCT *tp, + TC_STRUCT *prev_tc, + TC_STRUCT *tc, + TC_STRUCT *blend_tc) +{ + tp_err_t res_create = TP_ERR_FAIL; + blend_type_t blend_requested = tpCheckBlendArcType(prev_tc, tc); + + switch (blend_requested) { + case BLEND_LINE_LINE: + res_create = tpCreateLineLineBlend(tp, prev_tc, tc, blend_tc); + break; + case BLEND_LINE_ARC: + res_create = tpCreateLineArcBlend(tp, prev_tc, tc, blend_tc); + break; + case BLEND_ARC_LINE: + res_create = tpCreateArcLineBlend(tp, prev_tc, tc, blend_tc); + break; + case BLEND_ARC_ARC: + res_create = tpCreateArcArcBlend(tp, prev_tc, tc, blend_tc); + break; + case BLEND_NONE: + default: + tp_debug_print("intersection type not recognized, aborting arc\n"); + res_create = TP_ERR_FAIL; + break; + } + + return res_create == TP_ERR_OK; +} + + +/** + * Handle creating a blend arc when a new line segment is about to enter the queue. + * This function handles the checks, setup, and calculations for creating a new + * blend arc. Essentially all of the blend arc functions are called through + * here to isolate the process. + */ +STATIC tc_blend_type_t tpHandleBlendArc(TP_STRUCT * const tp, TC_STRUCT * const tc) { + + tp_debug_print("*****************************************\n** Handle Blend Arc **\n"); + + TC_STRUCT *prev_tc; + prev_tc = tcqLast(&tp->queue); + + //If the previous segment has already started, then don't create a blend + //arc for the next pair. + // TODO May be able to lift this restriction if we can ensure that we leave + // 1 timestep's worth of distance in prev_tc + if ( !prev_tc) { + tp_debug_print(" queue empty\n"); + return NO_BLEND; + } + if (prev_tc->progress > prev_tc->target / 2.0) { + tp_debug_print(" prev_tc progress (%f) is too large, aborting blend arc\n", prev_tc->progress); + return NO_BLEND; + } + + // Check for tangency between segments and handle any errors + // TODO possibly refactor this into a macro? + int res_tan = tpSetupTangent(tp, prev_tc, tc); + switch (res_tan) { + // Abort blend arc creation in these cases + case TP_ERR_FAIL: + tp_debug_print(" tpSetupTangent failed, aborting blend arc\n"); + case TP_ERR_OK: + return res_tan; + case TP_ERR_NO_ACTION: + default: + //Continue with creation + break; + } + + TC_STRUCT blend_tc = {0}; + + tc_blend_type_t blend_used = NO_BLEND; + + bool arc_blend_ok = tpCreateBlendIfPossible(tp, prev_tc, tc, &blend_tc); + + if (arc_blend_ok) { + //Need to do this here since the length changed + blend_used = ARC_BLEND; + tpAddSegmentToQueue(tp, &blend_tc, false); + } else { + // If blend arc creation failed early on, catch it here and find the best blend + blend_used = tpChooseBestBlend(tp, prev_tc, tc, NULL) ; + } + + return blend_used; +} + +//TODO final setup steps as separate functions +// +/** + * Add a straight line to the tc queue. + * end of the previous move to the new end specified here at the + * currently-active accel and vel settings from the tp struct. + */ + +int tpAddLine(TP_STRUCT * const tp, EmcPose end, int canon_motion_type, + double vel, double ini_maxvel, double acc, double ini_maxjerk, unsigned char enables, + char atspeed, int indexer_jnum, struct state_tag_t tag) +{ + if (tpErrorCheck(tp) < 0) { + return TP_ERR_FAIL; + } + tp_info_print("== AddLine ==\n"); + + // Initialize new tc struct for the line segment + TC_STRUCT tc = {0}; + tcInit(&tc, + TC_LINEAR, + canon_motion_type, + tp->cycleTime, + enables, + atspeed); + tc.tag = tag; + + // Setup any synced IO for this move + tpSetupSyncedIO(tp, &tc); + + // Copy over state data from the trajectory planner + tcSetupState(&tc, tp); + + // Copy in motion parameters + tcSetupMotion(&tc, + vel, + ini_maxvel, + acc, + ini_maxjerk); + // Setup line geometry + pmLine9Init(&tc.coords.line, + &tp->goalPos, + &end); + tc.target = pmLine9Target(&tc.coords.line); + if (tc.target < TP_POS_EPSILON) { + rtapi_print_msg(RTAPI_MSG_DBG,"failed to create line id %d, zero-length segment\n",tp->nextId); + return TP_ERR_ZERO_LENGTH; + } + tc.nominal_length = tc.target; + tcClampVelocityByLength(&tc); + + // For linear move, set joint corresponding to a locking indexer axis + tc.indexer_jnum = indexer_jnum; + + //TODO refactor this into its own function + TC_STRUCT *prev_tc; + prev_tc = tcqLast(&tp->queue); + handleModeChange(prev_tc, &tc); + if (emcmotConfig->arcBlendEnable){ + tpHandleBlendArc(tp, &tc); + } + tcFinalizeLength(prev_tc); + tcFlagEarlyStop(prev_tc, &tc); + + int retval = tpAddSegmentToQueue(tp, &tc, true); + //Run speed optimization (will abort safely if there are no tangent segments) + tpRunOptimization(tp); + + return retval; +} + + +/** + * Adds a circular (circle, arc, helix) move from the end of the + * last move to this new position. + * + * @param end is the xyz/abc point of the destination. + * + * see pmCircleInit for further details on how arcs are specified. Note that + * degenerate arcs/circles are not allowed. We are guaranteed to have a move in + * xyz so the target is always the circle/arc/helical length. + */ +int tpAddCircle(TP_STRUCT * const tp, + EmcPose end, + PmCartesian center, + PmCartesian normal, + int turn, + int canon_motion_type, + double vel, + double ini_maxvel, + double acc, + double ini_maxjerk, + unsigned char enables, + char atspeed, + struct state_tag_t tag) +{ + if (tpErrorCheck(tp)<0) { + return TP_ERR_FAIL; + } + + tp_info_print("== AddCircle ==\n"); + tp_debug_print("ini_maxvel = %f\n",ini_maxvel); + + TC_STRUCT tc = {0}; + + tcInit(&tc, + TC_CIRCULAR, + canon_motion_type, + tp->cycleTime, + enables, + atspeed); + tc.tag = tag; + // Setup any synced IO for this move + tpSetupSyncedIO(tp, &tc); + + // Copy over state data from the trajectory planner + tcSetupState(&tc, tp); + + // Setup circle geometry + int res_init = pmCircle9Init(&tc.coords.circle, + &tp->goalPos, + &end, + ¢er, + &normal, + turn); + + if (res_init) return res_init; + + // Update tc target with existing circular segment + tc.target = pmCircle9Target(&tc.coords.circle); + if (tc.target < TP_POS_EPSILON) { + return TP_ERR_ZERO_LENGTH; + } + tp_debug_print("tc.target = %f\n",tc.target); + tc.nominal_length = tc.target; + + // Copy in motion parameters + tcSetupMotion(&tc, + vel, + ini_maxvel, + acc, + ini_maxjerk); + + //Reduce max velocity to match sample rate + tcClampVelocityByLength(&tc); + + // Apply acceleration and jerk limits for circular motion + tcUpdateArcLimits(&tc); + + TC_STRUCT *prev_tc; + prev_tc = tcqLast(&tp->queue); + + handleModeChange(prev_tc, &tc); + if (emcmotConfig->arcBlendEnable){ + tpHandleBlendArc(tp, &tc); + findSpiralArcLengthFit(&tc.coords.circle.xyz, &tc.coords.circle.fit); + } + tcFinalizeLength(prev_tc); + tcFlagEarlyStop(prev_tc, &tc); + + int retval = tpAddSegmentToQueue(tp, &tc, true); + + tpRunOptimization(tp); + return retval; +} + + +/** + * Adjusts blend velocity and acceleration to safe limits. + * If we are blending between tc and nexttc, then we need to figure out what a + * safe blend velocity is based on the known trajectory parameters. This + * function updates the TC_STRUCT data with a safe blend velocity. + * + * @note This function will compute the parabolic blend start / end velocities + * regardless of the current terminal condition (useful for planning). + */ +STATIC int tpComputeBlendVelocity( + TC_STRUCT const *tc, + TC_STRUCT const *nexttc, + double target_vel_this, + double target_vel_next, + double *v_blend_this, + double *v_blend_next, + double *v_blend_net) +{ + /* Pre-checks for valid pointers */ + if (!nexttc || !tc || !v_blend_this || !v_blend_next ) { + return TP_ERR_FAIL; + } + + double acc_this = tcGetTangentialMaxAccel(tc); + double acc_next = tcGetTangentialMaxAccel(nexttc); + + double v_reachable_this = fmin(tpCalculateTriangleVel(tc), target_vel_this); + double v_reachable_next = fmin(tpCalculateTriangleVel(nexttc), target_vel_next); + + /* Compute the maximum allowed blend time for each segment. + * This corresponds to the minimum acceleration that will just barely reach + * max velocity as we are 1/2 done the segment. + */ + + double t_max_this = tc->target / v_reachable_this; + double t_max_next = nexttc->target / v_reachable_next; + double t_max_reachable = fmin(t_max_this, t_max_next); + + // How long the blend phase would be at maximum acceleration + double t_min_blend_this = v_reachable_this / acc_this; + double t_min_blend_next = v_reachable_next / acc_next; + + double t_max_blend = fmax(t_min_blend_this, t_min_blend_next); + // The longest blend time we can get that's still within the 1/2 segment restriction + double t_blend = fmin(t_max_reachable, t_max_blend); + + // Now, use this blend time to find the best acceleration / velocity for each segment + *v_blend_this = fmin(v_reachable_this, t_blend * acc_this); + *v_blend_next = fmin(v_reachable_next, t_blend * acc_next); + + double theta; + + PmCartesian v1, v2; + + tcGetEndAccelUnitVector(tc, &v1); + tcGetStartAccelUnitVector(nexttc, &v2); + findIntersectionAngle(&v1, &v2, &theta); + + double cos_theta = cos(theta); + + if (tc->tolerance > 0) { + /* see diagram blend.fig. T (blend tolerance) is given, theta + * is calculated from dot(s1, s2) + * + * blend criteria: we are decelerating at the end of segment s1 + * and we pass distance d from the end. + * find the corresponding velocity v when passing d. + * + * in the drawing note d = 2T/cos(theta) + * + * when v1 is decelerating at a to stop, v = at, t = v/a + * so required d = .5 a (v/a)^2 + * + * equate the two expressions for d and solve for v + */ + double tblend_vel; + /* Minimum value of cos(theta) to prevent numerical instability */ + const double min_cos_theta = cos(PM_PI / 2.0 - TP_MIN_ARC_ANGLE); + if (cos_theta > min_cos_theta) { + tblend_vel = 2.0 * pmSqrt(acc_this * tc->tolerance / cos_theta); + *v_blend_this = fmin(*v_blend_this, tblend_vel); + *v_blend_next = fmin(*v_blend_next, tblend_vel); + } + } + if (v_blend_net) { + /* + * Find net velocity in the direction tangent to the blend. + * When theta ~ 0, net velocity in tangent direction is very small. + * When the segments are nearly tangent (theta ~ pi/2), the blend + * velocity is almost entirely in the tangent direction. + */ + *v_blend_net = sin(theta) * (*v_blend_this + *v_blend_next) / 2.0; + } + + return TP_ERR_OK; +} + +/** + * Adjusts blend velocity and acceleration to safe limits. + * If we are blending between tc and nexttc, then we need to figure out what a + * safe blend velocity is based on the known trajectory parameters. This + * function updates the TC_STRUCT data with a safe blend velocity. + * + * @note This function will compute the parabolic blend start / end velocities + * regardless of the current terminal condition (useful for planning). + */ +STATIC int tpComputeBlendSCurveVelocity( + TC_STRUCT const *tc, + TC_STRUCT const *nexttc, + double target_vel_this, + double target_vel_next, + double *v_blend_this, + double *v_blend_next, + double *v_blend_net) +{ + /* Pre-checks for valid pointers */ + if (!nexttc || !tc || !v_blend_this || !v_blend_next ) { + return TP_ERR_FAIL; + } + + double acc_this = tcGetTangentialMaxAccel(tc); + double acc_next = tcGetTangentialMaxAccel(nexttc); + + double v_reachable_this = fmin(tpCalculateSCurveVel(tc), target_vel_this); + double v_reachable_next = fmin(tpCalculateSCurveVel(nexttc), target_vel_next); + + //double maxjerk = tc->maxjerk; + double maxjerk = fmin(tc->maxjerk, emcmotStatus->jerk); + /* Compute the maximum allowed blend time for each segment. + * This corresponds to the minimum acceleration that will just barely reach + * max velocity as we are 1/2 done the segment. + */ + + double t_max_this = tc->target / v_reachable_this; + double t_max_next = nexttc->target / v_reachable_next; + double t_max_reachable = fmin(t_max_this, t_max_next); + + // How long the blend phase would be at maximum acceleration + double t_min_blend_this; + double t_min_blend_next; + double t1_this, t2_this; + double t1_next, t2_next; + t_min_blend_this = calcDecelerateTimes(v_reachable_this, acc_this, emcmotStatus->jerk, &t1_this, &t2_this); + t_min_blend_next = calcDecelerateTimes(v_reachable_next, acc_next, emcmotStatus->jerk, &t1_next, &t2_next); + + + double t_max_blend = fmax(t_min_blend_this, t_min_blend_next); + // The longest blend time we can get that's still within the 1/2 segment restriction + double t_blend = fmin(t_max_reachable, t_max_blend); + + // Now, use this blend time to find the best acceleration / velocity for each segment + // For S-curve, we need to use calcSCurveSpeedWithT instead of simple t * acc + // calcSCurveSpeedWithT calculates the speed achievable in time T using S-curve acceleration + double v_blend_from_time_this = calcSCurveSpeedWithT(acc_this, maxjerk, t_blend); + double v_blend_from_time_next = calcSCurveSpeedWithT(acc_next, maxjerk, t_blend); + *v_blend_this = fmin(v_reachable_this, v_blend_from_time_this); + *v_blend_next = fmin(v_reachable_next, v_blend_from_time_next); + + double theta; + + PmCartesian v1, v2; + + tcGetEndAccelUnitVector(tc, &v1); + tcGetStartAccelUnitVector(nexttc, &v2); + findIntersectionAngle(&v1, &v2, &theta); + + double cos_theta = cos(theta); + + if (tc->tolerance > 0) { + /* see diagram blend.fig. T (blend tolerance) is given, theta + * is calculated from dot(s1, s2) + * + * blend criteria: we are decelerating at the end of segment s1 + * and we pass distance d from the end. + * find the corresponding velocity v when passing d. + * + * in the drawing note d = 2T/cos(theta) + * + * when v1 is decelerating at a to stop, v = at, t = v/a + * so required d = .5 a (v/a)^2 + * + * equate the two expressions for d and solve for v + */ + double tblend_vel; + /* Minimum value of cos(theta) to prevent numerical instability */ + const double min_cos_theta = cos(PM_PI / 2.0 - TP_MIN_ARC_ANGLE); + if (cos_theta > min_cos_theta && maxjerk > 0.0) { + /* For S-curve, the distance d = 2T/cos(theta) where T is tolerance. + * For S-curve deceleration from velocity v to 0: + * - Distance s = (1/6) * J * T1^3 (for triangle case, no S2 segment) + * where T1 = sqrt(v/J), so s = (1/6) * v^(3/2) / sqrt(J) + * - Solving for v: v = (6 * s * sqrt(J))^(2/3) + * + * Using the triangle case formula as an approximation (lower bound). + */ + double d = 2.0 * tc->tolerance / cos_theta; + tblend_vel = pow(6.0 * d * pmSqrt(maxjerk), 2.0 / 3.0); + *v_blend_this = fmin(*v_blend_this, tblend_vel); + *v_blend_next = fmin(*v_blend_next, tblend_vel); + } + } + if (v_blend_net) { + /* + * Find net velocity in the direction tangent to the blend. + * When theta ~ 0, net velocity in tangent direction is very small. + * When the segments are nearly tangent (theta ~ pi/2), the blend + * velocity is almost entirely in the tangent direction. + */ + *v_blend_net = sin(theta) * (*v_blend_this + *v_blend_next) / 2.0; + } + + return TP_ERR_OK; +} + +STATIC double estimateParabolicBlendPerformance( + TP_STRUCT const *tp, + TC_STRUCT const *tc, + TC_STRUCT const *nexttc) +{ + double v_this = 0.0, v_next = 0.0; + + // Use maximum possible target velocity to get best-case performance + double target_vel_this = tpGetMaxTargetVel(tp, tc); + double target_vel_next = tpGetMaxTargetVel(tp, nexttc); + + double v_net = 0.0; + if(GET_TRAJ_PLANNER_TYPE() == 1) + tpComputeBlendSCurveVelocity(tc, nexttc, target_vel_this, target_vel_next, &v_this, &v_next, &v_net); + else + tpComputeBlendVelocity(tc, nexttc, target_vel_this, target_vel_next, &v_this, &v_next, &v_net); + + return v_net; +} + + + +/** + * Calculate distance update from velocity and acceleration. + */ +STATIC int tcUpdateDistFromAccel(TC_STRUCT * const tc, double acc, double vel_desired, int reverse_run) +{ + // If the resulting velocity is less than zero, than we're done. This + // causes a small overshoot, but in practice it is very small. + //double v_next = tc->currentvel + acc * tc->cycle_time; + double v_next; + int planner_type = GET_TRAJ_PLANNER_TYPE(); + if(planner_type == 1) planner_type = 0; // if is 1, and inside here. it's means the jerk less than 1 + + v_next = tc->currentvel + acc * tc->cycle_time; + // update position in this tc using trapezoidal integration + // Note that progress can be greater than the target after this step. + //if (v_next < 0.0) { + if (planner_type == 0 && v_next < 0.0) { + v_next = 0.0; + //KLUDGE: the trapezoidal planner undershoots by half a cycle time, so + //forcing the endpoint here is necessary. However, velocity undershoot + //also occurs during pausing and stopping, which can happen far from + //the end. If we could "cruise" to the endpoint within a cycle at our + //current speed, then assume that we want to be at the end. + if (tcGetDistanceToGo(tc,reverse_run) < (tc->currentvel * tc->cycle_time)) { + tc->progress = tcGetTarget(tc,reverse_run); + } + } else { + double displacement = (v_next + tc->currentvel) * 0.5 * tc->cycle_time; + // Account for reverse run (flip sign if need be) + double disp_sign = reverse_run ? -1 : 1; + if(planner_type == 0) + tc->progress += (disp_sign * displacement); + + //Progress has to be within the allowable range + tc->progress = bisaturate(tc->progress, tcGetTarget(tc, TC_DIR_FORWARD), tcGetTarget(tc, TC_DIR_REVERSE)); + } + // Calculate jerk as rate of change of acceleration (for trapezoidal, this is high) + double jerk = 0.0; + if (tc->cycle_time > TP_TIME_EPSILON) { + jerk = (acc - tc->currentacc) / tc->cycle_time; + } + + if(planner_type == 0){ + tc->currentvel = v_next; + tc->currentacc = acc; + tc->currentjerk = jerk; + + // Check if we can make the desired velocity + tc->on_final_decel = (fabs(vel_desired - tc->currentvel) < TP_VEL_EPSILON) && (acc < 0.0); + }else{ + // Check if we can make the desired velocity + tc->on_final_decel = (fabs(vel_desired - tc->currentvel) < TP_VEL_EPSILON) && (acc <= 0.0); + tc->currentvel = v_next; + tc->currentacc = acc; + tc->currentjerk = jerk; + } + return TP_ERR_OK; +} + +STATIC void tpDebugCycleInfo(TP_STRUCT const * const tp, TC_STRUCT const * const tc, TC_STRUCT const * const nexttc, double acc) { +#ifdef TC_DEBUG + // Find maximum allowed velocity from feed and machine limits + double tc_target_vel = tpGetRealTargetVel(tp, tc); + // Store a copy of final velocity + double tc_finalvel = tpGetRealFinalVel(tp, tc, nexttc); + + /* Debug Output */ + tc_debug_print("tc state: vr = %f, vf = %f, maxvel = %f\n", + tc_target_vel, tc_finalvel, tc->maxvel); + tc_debug_print(" currentvel = %f, fs = %f, tc = %f, term = %d\n", + tc->currentvel, tpGetFeedScale(tp,tc), tc->cycle_time, tc->term_cond); + tc_debug_print(" acc = %f, T = %f, DTG = %.12g\n", acc, + tcGetTarget(tc,tp->reverse_run), tcGetDistanceToGo(tc,tp->reverse_run)); + tc_debug_print(" reverse_run = %d\n", tp->reverse_run); + tc_debug_print(" motion type %d\n", tc->motion_type); + + if (tc->on_final_decel) { + rtapi_print(" on final decel\n"); + } +#else + (void)tp; + (void)tc; + (void)nexttc; + (void)acc; +#endif +} + +/** + * Compute updated position and velocity for a timestep based on a trapezoidal + * motion profile. + * @param tc trajectory segment being processed. + * + * Creates the trapezoidal velocity profile based on the segment's velocity and + * acceleration limits. The formula has been tweaked slightly to allow a + * non-zero velocity at the instant the target is reached. + */ +void tpCalculateTrapezoidalAccel(TP_STRUCT const * const tp, TC_STRUCT * const tc, TC_STRUCT const * const nexttc, + double * const acc, double * const vel_desired) +{ + tc_debug_print("using trapezoidal acceleration\n"); + + // Find maximum allowed velocity from feed and machine limits + double tc_target_vel = tpGetRealTargetVel(tp, tc); + // Store a copy of final velocity + double tc_finalvel = tpGetRealFinalVel(tp, tc, nexttc); + +#ifdef TP_PEDANTIC + if (tc_finalvel > 0.0 && tc->term_cond != TC_TERM_COND_TANGENT) { + rtapi_print_msg(RTAPI_MSG_ERR, "Final velocity of %f with non-tangent segment!\n",tc_finalvel); + tc_finalvel = 0.0; + } +#endif + + /* Calculations for desired velocity based on trapezoidal profile */ + double dx = tcGetDistanceToGo(tc, tp->reverse_run); + double maxaccel = tcGetTangentialMaxAccel(tc); + + double discr_term1 = pmSq(tc_finalvel); + double discr_term2 = maxaccel * (2.0 * dx - tc->currentvel * tc->cycle_time); + double tmp_adt = maxaccel * tc->cycle_time * 0.5; + double discr_term3 = pmSq(tmp_adt); + + double discr = discr_term1 + discr_term2 + discr_term3; + + // Discriminant is a little more complicated with final velocity term. If + // discriminant < 0, we've overshot (or are about to). Do the best we can + // in this situation +#ifdef TP_PEDANTIC + if (discr < 0.0) { + rtapi_print_msg(RTAPI_MSG_ERR, + "discriminant %f < 0 in velocity calculation!\n", discr); + } +#endif + //Start with -B/2 portion of quadratic formula + double maxnewvel = -tmp_adt; + + //If the discriminant term brings our velocity above zero, add it to the total + //We can ignore the calculation otherwise because negative velocities are clipped to zero + if (discr > discr_term3) { + maxnewvel += pmSqrt(discr); + } + + // Find bounded new velocity based on target velocity + // Note that we use a separate variable later to check if we're on final decel + double newvel = saturate(maxnewvel, tc_target_vel); + + // Calculate acceleration needed to reach newvel, bounded by machine maximum + double dt = fmax(tc->cycle_time, TP_TIME_EPSILON); + double maxnewaccel = (newvel - tc->currentvel) / dt; + *acc = saturate(maxnewaccel, maxaccel); + *vel_desired = maxnewvel; +} + +/** + * Calculate "ramp" acceleration for a cycle. + */ +STATIC int tpCalculateRampAccel(TP_STRUCT const * const tp, + TC_STRUCT * const tc, + TC_STRUCT const * const nexttc, + double * const acc, + double * const vel_desired) +{ + tc_debug_print("using ramped acceleration\n"); + // displacement remaining in this segment + double dx = tcGetDistanceToGo(tc, tp->reverse_run); + + if (!tc->blending_next) { + tc->vel_at_blend_start = tc->currentvel; + } + + double vel_final = tpGetRealFinalVel(tp, tc, nexttc); + + /* Check if the final velocity is too low to properly ramp up.*/ + if (vel_final < TP_VEL_EPSILON) { + tp_debug_print(" vel_final %f too low for velocity ramping\n", vel_final); + return TP_ERR_FAIL; + } + + double vel_avg = (tc->currentvel + vel_final) / 2.0; + + // Calculate time remaining in this segment assuming constant acceleration + double dt = 1e-16; + if (vel_avg > TP_VEL_EPSILON) { + dt = fmax( dx / vel_avg, 1e-16); + } + + // Calculate velocity change between final and current velocity + double dv = vel_final - tc->currentvel; + + // Estimate constant acceleration required + double acc_final = dv / dt; + + // Saturate estimated acceleration against maximum allowed by segment + double acc_max = tcGetTangentialMaxAccel(tc); + + // Output acceleration and velocity for position update + *acc = saturate(acc_final, acc_max); + *vel_desired = vel_final; + + return TP_ERR_OK; +} + +/** + * Calculate distance update from velocity and acceleration. + */ +STATIC int tcUpdateDistFromSCurveAccel(TC_STRUCT *const tc, double acc, double jerk, double vel_desired, double perror __attribute__((unused)), int reverse_run, int dec, double req_pos_value) +{ + double v_next = vel_desired; + + // Handle negative velocity: only force to 0 when near endpoint (numerical error) + // If there is remaining distance, motion is still in progress + if (v_next < 0.0) { + double dx = tcGetDistanceToGo(tc, reverse_run); + if (dx < TP_POS_EPSILON) { + if (dx < (tc->currentvel * tc->cycle_time)) { + tc->progress = tcGetTarget(tc, reverse_run); + } + } + } else { + double disp_sign = reverse_run ? -1 : 1; + + // If req_pos provided, use Ruckig's exact position directly + // Otherwise use trapezoidal integration (fallback method) + if (req_pos_value >= 0.0) { + // Use Ruckig-computed exact position + tc->progress = req_pos_value; + double displacement = req_pos_value - (tc->progress - (tc->currentvel + v_next) * tc->cycle_time / 2.0 * disp_sign); + tc->last_move_length = fabs(displacement); + } else { + // Use trapezoidal integration for displacement + double displacement = (tc->currentvel + v_next) * tc->cycle_time / 2.0; + + tc->last_move_length = displacement; + tc->progress += (disp_sign * displacement); + } + + // Progress has to be within the allowable range + tc->progress = bisaturate(tc->progress, tcGetTarget(tc, TC_DIR_FORWARD), tcGetTarget(tc, TC_DIR_REVERSE)); + } + + tc->currentvel = v_next; + tc->currentacc = acc; + tc->currentjerk = jerk; + tc->on_final_decel = dec; + + return TP_ERR_OK; +} + + +/** + * Compute updated position and velocity for a timestep based on a s-curve + * motion profile. + * @param tc trajectory segment being processed. + * + * Creates the s-curve velocity profile based on the segment's velocity and + * acceleration limits. The formula has been tweaked slightly to allow a + * non-zero velocity at the instant the target is reached. + */ +int tpCalculateSCurveAccel(TP_STRUCT const * const tp, TC_STRUCT * const tc, TC_STRUCT const * const nexttc, + double * const acc, double * const jerk, double * const vel_desired, double * const pos_error, int blend, double * const req_pos) +{ + tc_debug_print("using s-curve acceleration with Ruckig\n"); + + double maxjerk = fmin(tc->maxjerk, emcmotStatus->jerk); + if(maxjerk <= 1){ + maxjerk = 1; + rtapi_print_msg(RTAPI_MSG_ERR, "ERROR!!! maxjerk Is less than 1\n"); + return TP_SCURVE_ACCEL_ERROR; + } + + // Find maximum allowed velocity from feed and machine limits + double tc_target_vel = tpGetRealTargetVel(tp, tc); + // Store a copy of final velocity + double tc_finalvel = tpGetRealFinalVel(tp, tc, nexttc); + + double dx = tcGetDistanceToGo(tc, tp->reverse_run); + double maxaccel = tcGetTangentialMaxAccel(tc); + + *pos_error = 0; + if(!blend && tc->cycle_time < TP_TIME_EPSILON){ + *acc = tc->currentacc; + *vel_desired = tc->currentvel; + *jerk = tc->currentjerk; + return TP_SCURVE_ACCEL_ACCEL; + } + + // Check pause/abort state + bool is_pausing = tp->pausing && (tc->synchronized == TC_SYNC_NONE || tc->synchronized == TC_SYNC_VELOCITY); + bool is_aborting = tp->aborting; + + // Check if feed_override = 0 (not pause/abort, but velocity limited to 0) + bool use_velocity_control = (is_pausing || is_aborting || + emcmotStatus->net_feed_scale <= TP_VEL_EPSILON); + // Normal operation parameters + double effective_max_vel = tc_target_vel; + double effective_target_vel = tc_finalvel; + + // ========== Ruckig trajectory planning ========== + + // Check if planner needs to be created or replanned + if (!tc->ruckig_planner) { + // Create Ruckig planner + tc->ruckig_planner = ruckig_create(tc->cycle_time); + if (!tc->ruckig_planner) { + rtapi_print_msg(RTAPI_MSG_ERR, "tpCalculateSCurveAccel: failed to create Ruckig planner\n"); + return TP_SCURVE_ACCEL_ERROR; + } + tc->ruckig_planned = 0; + tc->ruckig_trajectory_time = 0.0; + tc->ruckig_last_maxaccel = 0.0; + tc->ruckig_last_maxjerk = 0.0; + tc->ruckig_last_target_vel = 0.0; + tc->ruckig_last_final_vel = 0.0; + tc->ruckig_last_target_pos = 0.0; + tc->ruckig_last_use_velocity_control = 0; + tc->ruckig_last_req_pos = 0.0; + tc->ruckig_last_feed_override = 0.0; + } + + // Check if parameters have changed (use small tolerance for float comparison) + const double PARAM_EPSILON = 1e-8; + int need_replan = 0; + + if (use_velocity_control) { + // Check if control mode changed (priority check: mode switch requires replanning) + if (tc->ruckig_planned && tc->ruckig_last_use_velocity_control != 1) { + // Switching from position to velocity control mode, must replan + need_replan = 1; + tc_debug_print("tpCalculateSCurveAccel: control mode changed from position to velocity, replanning\n"); + rtapi_print_msg(RTAPI_MSG_DBG, "tpCalculateSCurveAccel: mode switch detected: last_mode=%d, current_mode=velocity\n", + tc->ruckig_last_use_velocity_control); + ruckig_reset(tc->ruckig_planner); + tc->ruckig_planned = 0; + } else if (!tc->ruckig_planned) { + need_replan = 1; + } else { + // Check key parameters (velocity control mode: only check velocity/accel params) + int param_changed = (fabs(tc->ruckig_last_maxaccel - maxaccel) > PARAM_EPSILON || + fabs(tc->ruckig_last_maxjerk - maxjerk) > PARAM_EPSILON || + fabs(tc->ruckig_last_final_vel - 0.0) > PARAM_EPSILON); + + if (param_changed) { + need_replan = 1; + } + } + + // Replan using velocity control mode + if (need_replan) { + double replan_vel = tc->currentvel; + double replan_acc = tc->currentacc; + + int plan_result = ruckig_plan_velocity(tc->ruckig_planner, + replan_vel, // current velocity + replan_acc, // current acceleration + 0.0, // target velocity (stop) + 0.0, // target acceleration (stop) + 0.0, // min velocity (unidirectional) + maxaccel, // max acceleration + maxjerk); // max jerk + + if (plan_result != 0) { + if (tc->ruckig_planned) { + rtapi_print_msg(RTAPI_MSG_WARN, "tpCalculateSCurveAccel: Ruckig velocity control replanning failed, using previous trajectory\n"); + } else { + rtapi_print_msg(RTAPI_MSG_WARN, "tpCalculateSCurveAccel: Ruckig velocity control planning failed, falling back to tp 0\n"); + return TP_SCURVE_ACCEL_ERROR; + } + } else { + tc->ruckig_planned = 1; + tc->ruckig_trajectory_time = 0.0; + tc->ruckig_last_maxaccel = maxaccel; + tc->ruckig_last_maxjerk = maxjerk; + tc->ruckig_last_target_vel = 0.0; + tc->ruckig_last_final_vel = 0.0; + tc->ruckig_last_target_pos = 0.0; + tc->ruckig_last_use_velocity_control = 1; + tc->ruckig_last_req_pos = 0.0; + tc->ruckig_last_feed_override = emcmotStatus->net_feed_scale; + } + } + } else { + // Position control mode: needs target position + double current_pos = tc->progress; + double target_pos = current_pos + dx; + + // Check if control mode changed + if (tc->ruckig_planned && tc->ruckig_last_use_velocity_control != 0) { + // Switching from velocity to position control mode, must replan + need_replan = 1; + rtapi_print_msg(RTAPI_MSG_DBG, "tpCalculateSCurveAccel: mode switch detected: last_mode=%d, current_mode=position\n", + tc->ruckig_last_use_velocity_control); + ruckig_reset(tc->ruckig_planner); + tc->ruckig_planned = 0; + } else if (!tc->ruckig_planned) { + need_replan = 1; + } else { + // Check key parameters (position control mode) + int param_changed = (fabs(tc->ruckig_last_maxaccel - maxaccel) > PARAM_EPSILON || + fabs(tc->ruckig_last_maxjerk - maxjerk) > PARAM_EPSILON || + fabs(tc->ruckig_last_target_vel - effective_max_vel) > PARAM_EPSILON || + fabs(tc->ruckig_last_final_vel - effective_target_vel) > PARAM_EPSILON || + fabs(tc->ruckig_last_target_pos - target_pos) > PARAM_EPSILON); + + if (param_changed) { + need_replan = 1; + } + } + + // Replan if needed + if (need_replan) { + double replan_pos = current_pos; + double replan_vel = tc->currentvel; + double replan_acc = tc->currentacc; + + int plan_result = ruckig_plan_position(tc->ruckig_planner, + replan_pos, // current position + replan_vel, // current velocity + replan_acc, // current acceleration + target_pos, // target position + effective_target_vel, // target velocity (finalvel) + 0.0, // target acceleration (usually 0) + 0.0, // min velocity (unidirectional) + effective_max_vel, // max velocity + maxaccel, // max acceleration + maxjerk); // max jerk + + if (plan_result != 0) { + rtapi_print_msg(RTAPI_MSG_INFO, "tpCalculateSCurveAccel: ruckig_plan_position failed with result %d\n", plan_result); + if (tc->ruckig_planned) { + // Keep using previous trajectory + } else { + // First planning attempt failed, fall back + rtapi_print_msg(RTAPI_MSG_ERR, + "Ruckig planning failed (first attempt), Back to tp 0\n" + " feed_override: %.6f \n" + " max_vel: %.6f\n" + " cpos: %.6f, tpos: %.6f, dx: %.6f\n" + " cvel: %.6f, tvel: %.6f\n" + " cacc: %.6f\n" + " maxa: %.6f, maxj: %.6f\n", + emcmotStatus->net_feed_scale, + effective_max_vel, + replan_pos, target_pos, dx, + replan_vel, effective_target_vel, + replan_acc, + maxaccel, maxjerk); + return TP_SCURVE_ACCEL_ERROR; + } + } else { + // Planning succeeded + rtapi_print_msg(RTAPI_MSG_DBG, "tpCalculateSCurveAccel: ruckig_plan_position succeeded\n"); + tc->ruckig_planned = 1; + tc->ruckig_trajectory_time = 0.0; + tc->ruckig_last_maxaccel = maxaccel; + tc->ruckig_last_maxjerk = maxjerk; + tc->ruckig_last_target_vel = effective_max_vel; + tc->ruckig_last_final_vel = effective_target_vel; + tc->ruckig_last_target_pos = target_pos; + tc->ruckig_last_use_velocity_control = 0; + tc->ruckig_last_req_pos = 0.0; + tc->ruckig_last_feed_override = emcmotStatus->net_feed_scale; + } + } else { + rtapi_print_msg(RTAPI_MSG_DBG, "tpCalculateSCurveAccel: no replan needed, using existing trajectory\n"); + } + } + + // Get next cycle state from Ruckig planning result + if (!tc->ruckig_planned) { + return TP_SCURVE_ACCEL_ERROR; + } + + double duration = ruckig_get_duration(tc->ruckig_planner); + double req_v = 0.0, req_a = 0.0, req_j = 0.0; + double req_pos_value = -1; + + int ruckig_result = ruckig_next_cycle(tc->ruckig_planner, + tc->ruckig_trajectory_time, + tc->cycle_time, + &req_pos_value, + &req_v, + &req_a, + &req_j); + + if (ruckig_result != 0) { + rtapi_print_msg(RTAPI_MSG_ERR, "tpCalculateSCurveAccel: Ruckig query failed, falling back\n"); + return TP_SCURVE_ACCEL_ERROR; + } + + // Update trajectory time + tc->ruckig_trajectory_time += tc->cycle_time; + + // Clamp trajectory time to duration to prevent unbounded growth on replan failure + if (duration > 0.0 && tc->ruckig_trajectory_time > duration) { + tc->ruckig_trajectory_time = duration; + } + + // Position coordinate system: + // - Velocity control mode: ruckig_plan_velocity sets initial position to 0, so Ruckig + // returns cumulative displacement from 0. Compute per-cycle delta and add to tc->progress. + // - Position control mode: ruckig_plan_position uses tc->progress as initial position, + // so Ruckig returns position directly in tc->progress coordinates. + if (use_velocity_control) { + // Velocity control: compute displacement delta from cumulative Ruckig position + double ruckig_original_pos = req_pos_value; + double displacement_delta = req_pos_value - tc->ruckig_last_req_pos; + double disp_sign = (tcGetTarget(tc, TC_DIR_FORWARD) > tcGetTarget(tc, TC_DIR_REVERSE)) ? 1 : -1; + req_pos_value = tc->progress + disp_sign * displacement_delta; + tc->ruckig_last_req_pos = ruckig_original_pos; + } else { + // Position control: Ruckig position is already in tc->progress coordinates + tc->ruckig_last_req_pos = 0.0; + } + + // Output results — trust Ruckig's S-curve planning + *acc = req_a; + *vel_desired = req_v; + *jerk = req_j; + if (req_pos) { + *req_pos = req_pos_value; + } + *pos_error = dx - (req_pos_value - tc->progress); + + // Determine acceleration vs deceleration phase + int res = (req_a < 0.0 || (req_a == 0.0 && req_v < tc->currentvel)) ? + TP_SCURVE_ACCEL_DECEL : TP_SCURVE_ACCEL_ACCEL; + + return res; +} + +void tpToggleDIOs(TC_STRUCT * const tc) { + + int i=0; + if (tc->syncdio.anychanged != 0) { // we have DIO's to turn on or off + for (i=0; i < emcmotConfig->numDIO; i++) { + if (!(tc->syncdio.dio_mask & (1 << i))) continue; + if (tc->syncdio.dios[i] > 0) _DioWrite(i, 1); // turn DIO[i] on + if (tc->syncdio.dios[i] < 0) _DioWrite(i, 0); // turn DIO[i] off + } + for (i=0; i < emcmotConfig->numAIO; i++) { + if (!(tc->syncdio.aio_mask & (1 << i))) continue; + _AioWrite(i, tc->syncdio.aios[i]); // set AIO[i] + } + tc->syncdio.anychanged = 0; //we have turned them all on/off, nothing else to do for this TC the next time + } +} + + +/** + * Handle special cases for rigid tapping. + * This function deals with updating the goal position and spindle position + * during a rigid tap cycle. In particular, the target and spindle goal need to + * be carefully handled since we're reversing direction. + */ +STATIC void tpUpdateRigidTapState(TP_STRUCT const * const tp, + TC_STRUCT * const tc) { + + static double old_spindlepos; + double new_spindlepos = emcmotStatus->spindle_status[tp->spindle.spindle_num].spindleRevs; + if (emcmotStatus->spindle_status[tp->spindle.spindle_num].direction < 0) + new_spindlepos = -new_spindlepos; + + switch (tc->coords.rigidtap.state) { + case RIGIDTAP_START: + old_spindlepos = new_spindlepos; + tc->coords.rigidtap.state = TAPPING; + /* Fallthrough */ + case TAPPING: + tc_debug_print("TAPPING\n"); + if (tc->progress >= tc->coords.rigidtap.reversal_target) { + // command reversal + emcmotStatus->spindle_status[tp->spindle.spindle_num].speed *= -1.0 * tc->coords.rigidtap.reversal_scale; + tc->coords.rigidtap.state = REVERSING; + } + break; + case REVERSING: + tc_debug_print("REVERSING\n"); + if (new_spindlepos < old_spindlepos) { + PmCartesian start, end; + PmCartLine *aux = &tc->coords.rigidtap.aux_xyz; + // we've stopped, so set a new target at the original position + tc->coords.rigidtap.spindlerevs_at_reversal = new_spindlepos + tp->spindle.offset; + + pmCartLinePoint(&tc->coords.rigidtap.xyz, tc->progress, &start); + end = tc->coords.rigidtap.xyz.start; + pmCartLineInit(aux, &start, &end); + rtapi_print_msg(RTAPI_MSG_DBG, "old target = %f", tc->target); + tc->coords.rigidtap.reversal_target = aux->tmag; + tc->target = aux->tmag + 10. * tc->uu_per_rev; + tc->progress = 0.0; + rtapi_print_msg(RTAPI_MSG_DBG, "new target = %f", tc->target); + + tc->coords.rigidtap.state = RETRACTION; + } + old_spindlepos = new_spindlepos; + tc_debug_print("Spindlepos = %f\n", new_spindlepos); + break; + case RETRACTION: + tc_debug_print("RETRACTION\n"); + if (tc->progress >= tc->coords.rigidtap.reversal_target) { + emcmotStatus->spindle_status[tp->spindle.spindle_num].speed *= -1 / tc->coords.rigidtap.reversal_scale; + tc->coords.rigidtap.state = FINAL_REVERSAL; + } + break; + case FINAL_REVERSAL: + tc_debug_print("FINAL_REVERSAL\n"); + if (new_spindlepos > old_spindlepos) { + PmCartesian start, end; + PmCartLine *aux = &tc->coords.rigidtap.aux_xyz; + pmCartLinePoint(aux, tc->progress, &start); + end = tc->coords.rigidtap.xyz.start; + pmCartLineInit(aux, &start, &end); + tc->target = aux->tmag; + tc->progress = 0.0; + //No longer need spindle sync at this point + tc->synchronized = 0; + tc->target_vel = tc->maxvel; + + tc->coords.rigidtap.state = FINAL_PLACEMENT; + } + old_spindlepos = new_spindlepos; + break; + case FINAL_PLACEMENT: + tc_debug_print("FINAL_PLACEMENT\n"); + // this is a regular move now, it'll stop at target above. + break; + } +} + + +/** + * Update emcMotStatus with information about trajectory motion. + * Based on the specified trajectory segment tc, read its progress and status + * flags. Then, update the emcmotStatus structure with this information. + */ +STATIC int tpUpdateMovementStatus(TP_STRUCT * const tp, TC_STRUCT const * const tc ) { + + + if (!tp) { + return TP_ERR_FAIL; + } + + if (!tc) { + // Assume that we have no active segment, so we should clear out the status fields + emcmotStatus->distance_to_go = 0; + emcmotStatus->enables_queued = emcmotStatus->enables_new; + emcmotStatus->requested_vel = 0; + emcmotStatus->current_vel = 0; + emcmotStatus->spindleSync = 0; + + // Clear S-curve motion state + emcmotStatus->current_acc = 0; + emcmotStatus->current_jerk = 0; + emcmotStatus->current_dir.x = 0; + emcmotStatus->current_dir.y = 0; + emcmotStatus->current_dir.z = 0; + + emcPoseZero(&emcmotStatus->dtg); + + tp->motionType = 0; + tp->activeDepth = 0; + return TP_ERR_STOPPED; + } + + EmcPose tc_pos; + tcGetEndpoint(tc, &tc_pos); + + tc_debug_print("tc id = %u canon_type = %u motion_type = %u\n", + tc->id, tc->canon_motion_type, tc->motion_type); + tp->motionType = tc->canon_motion_type; + tp->activeDepth = tc->active_depth; + emcmotStatus->distance_to_go = tc->target - tc->progress; + emcmotStatus->enables_queued = tc->enables; + // report our line number to the guis + tp->execId = tc->id; + emcmotStatus->requested_vel = tc->reqvel; + emcmotStatus->current_vel = tc->currentvel; + + // Output accurate S-curve motion state (for accurate jerk calculation) + emcmotStatus->current_acc = tc->currentacc; + emcmotStatus->current_jerk = tc->currentjerk; + + // Get current motion direction unit vector (precise tangent at current progress) + PmCartesian dir; + if (tcGetCurrentTangentUnitVector(tc, &dir) == 0) { + emcmotStatus->current_dir = dir; + } else { + // If direction unavailable, use zero vector + emcmotStatus->current_dir.x = 0; + emcmotStatus->current_dir.y = 0; + emcmotStatus->current_dir.z = 0; + } + + emcPoseSub(&tc_pos, &tp->currentPos, &emcmotStatus->dtg); + return TP_ERR_OK; +} + + +/** + * Do a parabolic blend by updating the nexttc. + * Perform the actual blending process by updating the target velocity for the + * next segment, then running a cycle update. + */ +STATIC void tpUpdateBlend(TP_STRUCT * const tp, TC_STRUCT * const tc, + TC_STRUCT * const nexttc) { + + if (!nexttc) { + return; + } + double save_vel = nexttc->target_vel; + bool is_abort = false; + + if (tpGetFeedScale(tp, nexttc) > TP_VEL_EPSILON) { + double dv = tc->vel_at_blend_start - tc->currentvel; + double vel_start = fmax(tc->vel_at_blend_start, TP_VEL_EPSILON); + // Clip the ratio at 1 and 0 + double blend_progress = fmax(fmin(dv / vel_start, 1.0), 0.0); + double blend_scale = tc->vel_at_blend_start / tc->blend_vel; + nexttc->target_vel = blend_progress * nexttc->blend_vel * blend_scale; + // Mark the segment as blending so we handle the new target velocity properly + nexttc->is_blending = true; + // Don't copy cycle_time - if tc has a partial split time, nexttc gets acc spikes + // nexttc->cycle_time = tc->cycle_time; + } else { + // Drive the target velocity to zero since we're stopping + nexttc->target_vel = 0.0; + save_vel = 0.0; + is_abort = true; + } + + int mode = 0; + if(is_abort) mode = 0; + else mode = 1; + + tpUpdateCycle(tp, nexttc, NULL, &mode); + //Restore the original target velocity + nexttc->target_vel = save_vel; +} + + +/** + * Cleanup if tc is not valid (empty queue). + * If the program ends, or we hit QUEUE STARVATION, do a soft reset on the trajectory planner. + * TODO merge with tpClear? + */ +STATIC void tpHandleEmptyQueue(TP_STRUCT * const tp) +{ + + tcqInit(&tp->queue); + tp->goalPos = tp->currentPos; + tp->done = 1; + tp->depth = tp->activeDepth = 0; + tp->aborting = 0; + tp->execId = 0; + tp->motionType = 0; + + tpUpdateMovementStatus(tp, NULL); + + tpResume(tp); +} + +/** Wrapper function to unlock rotary axes */ +STATIC void tpSetRotaryUnlock(int axis, int unlock) { + _SetRotaryUnlock(axis, unlock); +} + +/** Wrapper function to check rotary axis lock */ +STATIC int tpGetRotaryIsUnlocked(int axis) { + return _GetRotaryIsUnlocked(axis); +} + + +/** + * Cleanup after a trajectory segment is complete. + * If the current move is complete and we're not waiting on the spindle for + * const this move, then pop if off the queue and perform cleanup operations. + * Finally, get the next move in the queue. + */ +STATIC int tpCompleteSegment(TP_STRUCT * const tp, + TC_STRUCT * const tc) { + + if (tp->spindle.waiting_for_atspeed == tc->id) { + return TP_ERR_FAIL; + } + + // if we're synced, and this move is ending, save the + // spindle position so the next synced move can be in + // the right place. + if(tc->synchronized != TC_SYNC_NONE) { + tp->spindle.offset += tc->target / tc->uu_per_rev; + } else { + tp->spindle.offset = 0.0; + } + + if(tc->indexer_jnum != -1) { + // this was an indexing move, so before we remove it we must + // relock the joint for the locking indexer axis + tpSetRotaryUnlock(tc->indexer_jnum, 0); + // if it is now locked, fall through and remove the finished move. + // otherwise, just come back later and check again + if(tpGetRotaryIsUnlocked(tc->indexer_jnum)) + return TP_ERR_FAIL; + } + + //Clear status flags associated since segment is done + //TODO stuff into helper function? + tc->active = 0; + tc->remove = 0; + tc->is_blending = 0; + tc->splitting = 0; + tc->cycle_time = tp->cycleTime; + //Velocities are by definition zero for a non-active segment + tc->currentvel = 0.0; + tc->term_vel = 0.0; + + // Clean up Ruckig planner resources + tcCleanupRuckig(tc); + + //TODO make progress to match target? + // done with this move + if (tp->reverse_run) { + tcqBackStep(&tp->queue); + tp_debug_print("Finished reverse run of tc id %d\n", tc->id); + } else { + int res_pop = tcqPop(&tp->queue); + if (res_pop) rtapi_print_msg(RTAPI_MSG_ERR,"Got error %d from tcqPop!\n", res_pop); + tp_debug_print("Finished tc id %d\n", tc->id); + } + + return TP_ERR_OK; +} + + +/** + * Handle an abort command. + * Based on the current motion state, handle the consequences of an abort command. + */ +STATIC tp_err_t tpHandleAbort(TP_STRUCT * const tp, TC_STRUCT * const tc, + TC_STRUCT * const nexttc) { + + if(!tp->aborting) { + //Don't need to do anything if not aborting + return TP_ERR_NO_ACTION; + } + //If the motion has stopped, then it's safe to reset the TP struct. + if( MOTION_ID_VALID(tp->spindle.waiting_for_index) || + MOTION_ID_VALID(tp->spindle.waiting_for_atspeed) || + (tc->currentvel == 0.0 && (!nexttc || nexttc->currentvel == 0.0))) { + tcqInit(&tp->queue); + tp->goalPos = tp->currentPos; + tp->done = 1; + tp->depth = tp->activeDepth = 0; + tp->aborting = 0; + tp->execId = 0; + tp->motionType = 0; + tp->synchronized = 0; + tp->reverse_run = 0; + tp->spindle.waiting_for_index = MOTION_INVALID_ID; + tp->spindle.waiting_for_atspeed = MOTION_INVALID_ID; + tpResume(tp); + return TP_ERR_STOPPED; + } //FIXME consistent error codes + return TP_ERR_SLOWING; +} + + +/** + * Check if the spindle has reached the required speed for a move. + * Returns a "wait" code if the spindle needs to spin up before a move and it + * has not reached the requested speed, or the spindle index has not been + * detected. + */ +STATIC tp_err_t tpCheckAtSpeed(TP_STRUCT * const tp, TC_STRUCT * const tc) +{ + int s; + // this is no longer the segment we were waiting_for_index for + if (MOTION_ID_VALID(tp->spindle.waiting_for_index) && tp->spindle.waiting_for_index != tc->id) + { + rtapi_print_msg(RTAPI_MSG_ERR, + "Was waiting for index on motion id %d, but reached id %d\n", + tp->spindle.waiting_for_index, tc->id); + tp->spindle.waiting_for_index = MOTION_INVALID_ID; + } + + if (MOTION_ID_VALID(tp->spindle.waiting_for_atspeed) && tp->spindle.waiting_for_atspeed != tc->id) + { + + rtapi_print_msg(RTAPI_MSG_ERR, + "Was waiting for atspeed on motion id %d, but reached id %d\n", + tp->spindle.waiting_for_atspeed, tc->id); + tp->spindle.waiting_for_atspeed = MOTION_INVALID_ID; + } + + if (MOTION_ID_VALID(tp->spindle.waiting_for_atspeed)) { + for (s = 0; s < emcmotConfig->numSpindles; s++){ + if(!emcmotStatus->spindle_status[s].at_speed) { + // spindle is still not at the right speed, so wait another cycle + return TP_ERR_WAITING; + } + } + // not waiting any more + tp->spindle.waiting_for_atspeed = MOTION_INVALID_ID; + } + + if (MOTION_ID_VALID(tp->spindle.waiting_for_index)) { + if (emcmotStatus->spindle_status[tp->spindle.spindle_num].spindle_index_enable) { + /* haven't passed index yet */ + return TP_ERR_WAITING; + } else { + rtapi_print_msg(RTAPI_MSG_DBG, "Index seen on spindle %d\n", tp->spindle.spindle_num); + /* passed index, start the move */ + emcmotStatus->spindleSync = 1; + tp->spindle.waiting_for_index = MOTION_INVALID_ID; + tc->sync_accel = 1; + tp->spindle.revs = 0; + } + } + return TP_ERR_OK; +} + +/** + * "Activate" a segment being read for the first time. + * This function handles initial setup of a new segment read off of the queue + * for the first time. + */ +STATIC tp_err_t tpActivateSegment(TP_STRUCT * const tp, TC_STRUCT * const tc) { + + //Check if already active + if (!tc || tc->active) { + return TP_ERR_OK; + } + + if (!tp) { + return TP_ERR_MISSING_INPUT; + } + + if (tp->reverse_run && (tc->motion_type == TC_RIGIDTAP || tc->synchronized != TC_SYNC_NONE)) { + //Can't activate a segment with synced motion in reverse + return TP_ERR_REVERSE_EMPTY; + } + + /* Based on the INI setting for "cutoff frequency", this calculation finds + * short segments that can have their acceleration be simple ramps, instead + * of a trapezoidal motion. This leads to fewer jerk spikes, at a slight + * performance cost. + * */ + double cutoff_time = 1.0 / (fmax(emcmotConfig->arcBlendRampFreq, TP_TIME_EPSILON)); + + double length = tcGetDistanceToGo(tc, tp->reverse_run); + // Given what velocities we can actually reach, estimate the total time for the segment under ramp conditions + double segment_time = 2.0 * length / (tc->currentvel + fmin(tc->finalvel,tpGetRealTargetVel(tp,tc))); + + + if (segment_time < cutoff_time && + tc->canon_motion_type != EMC_MOTION_TYPE_TRAVERSE && + tc->term_cond == TC_TERM_COND_TANGENT && + tc->motion_type != TC_RIGIDTAP && + length != 0) + { + tp_debug_print("segment_time = %f, cutoff_time = %f, ramping\n", + segment_time, cutoff_time); + tc->accel_mode = TC_ACCEL_RAMP; + } + + // Do at speed checks that only happen once + int needs_atspeed = tc->atspeed || + (tc->synchronized == TC_SYNC_POSITION && !(emcmotStatus->spindleSync)); + + if (needs_atspeed){ + int s; + for (s = 0; s < emcmotConfig->numSpindles; s++){ + if (!emcmotStatus->spindle_status[s].at_speed) { + tp->spindle.waiting_for_atspeed = tc->id; + return TP_ERR_WAITING; + } + } + } + + if (tc->indexer_jnum != -1) { + // request that the joint for the locking indexer axis unlock + tpSetRotaryUnlock(tc->indexer_jnum, 1); + // if it is unlocked, fall through and start the move. + // otherwise, just come back later and check again + if (!tpGetRotaryIsUnlocked(tc->indexer_jnum)) { + return TP_ERR_WAITING; + } + } + + // Temporary debug message + tp_debug_print("Activate tc id = %d target_vel = %f req_vel = %f final_vel = %f length = %f\n", + tc->id, + tc->target_vel, + tc->reqvel, + tc->finalvel, + tc->target); + + tc->active = 1; + //Do not change initial velocity here, since tangent blending already sets this up + tp->motionType = tc->canon_motion_type; + tc->blending_next = 0; + tc->on_final_decel = 0; + + if (TC_SYNC_POSITION == tc->synchronized && !(emcmotStatus->spindleSync)) { + tp_debug_print("Setting up position sync\n"); + // if we aren't already synced, wait + tp->spindle.waiting_for_index = tc->id; + // ask for an index reset + emcmotStatus->spindle_status[tp->spindle.spindle_num].spindle_index_enable = 1; + tp->spindle.offset = 0.0; + rtapi_print_msg(RTAPI_MSG_DBG, "Waiting on sync. spindle_num %d..\n", tp->spindle.spindle_num); + return TP_ERR_WAITING; + } + + // Update the modal state displayed by the TP + tp->execTag = tc->tag; + + return TP_ERR_OK; +} + + +/** + * Run velocity mode synchronization. + * Update requested velocity to follow the spindle's velocity (scaled by feed rate). + */ +STATIC void tpSyncVelocityMode(TP_STRUCT * const tp, TC_STRUCT * const tc, TC_STRUCT * const nexttc) { + double speed = emcmotStatus->spindle_status[tp->spindle.spindle_num].spindleSpeedIn; + double pos_error = fabs(speed) * tc->uu_per_rev; + // Account for movement due to parabolic blending with next segment + if(nexttc) { + pos_error -= nexttc->progress; + } + tc->target_vel = pos_error; + + if (nexttc && nexttc->synchronized) { + //If the next move is synchronized too, then match it's + //requested velocity to the current move + nexttc->target_vel = tc->target_vel; + } +} + + +/** + * Run position mode synchronization. + * Updates requested velocity for a trajectory segment to track the spindle's position. + */ +STATIC void tpSyncPositionMode(TP_STRUCT * const tp, TC_STRUCT * const tc, + TC_STRUCT * const nexttc ) { + + double spindle_pos = tpGetSignedSpindlePosition(&emcmotStatus->spindle_status[tp->spindle.spindle_num]); + tp_debug_print("Spindle at %f\n",spindle_pos); + double spindle_vel, target_vel; + double oldrevs = tp->spindle.revs; + + if ((tc->motion_type == TC_RIGIDTAP) && (tc->coords.rigidtap.state == RETRACTION || + tc->coords.rigidtap.state == FINAL_REVERSAL)) { + tp->spindle.revs = tc->coords.rigidtap.spindlerevs_at_reversal - + spindle_pos; + } else { + tp->spindle.revs = spindle_pos; + } + + double pos_desired = (tp->spindle.revs - tp->spindle.offset) * tc->uu_per_rev; + double pos_error = pos_desired - tc->progress; + + if(nexttc) { + pos_error -= nexttc->progress; + } + + if(tc->sync_accel) { + // detect when velocities match, and move the target accordingly. + // acceleration will abruptly stop and we will be on our new target. + // FIX: this is driven by TP cycle time, not the segment cycle time + double dt = fmax(tp->cycleTime, TP_TIME_EPSILON); + spindle_vel = tp->spindle.revs / ( dt * tc->sync_accel++); + target_vel = spindle_vel * tc->uu_per_rev; + if(tc->currentvel >= target_vel) { + tc_debug_print("Hit accel target in pos sync\n"); + // move target so as to drive pos_error to 0 next cycle + tp->spindle.offset = tp->spindle.revs - tc->progress / tc->uu_per_rev; + tc->sync_accel = 0; + tc->target_vel = target_vel; + } else { + tc_debug_print("accelerating in pos_sync\n"); + // beginning of move and we are behind: accel as fast as we can + tc->target_vel = tc->maxvel; + } + } else { + // we have synced the beginning of the move as best we can - + // track position (minimize pos_error). + tc_debug_print("tracking in pos_sync\n"); + double errorvel; + spindle_vel = (tp->spindle.revs - oldrevs) / tp->cycleTime; + target_vel = spindle_vel * tc->uu_per_rev; + errorvel = pmSqrt(fabs(pos_error) * tcGetTangentialMaxAccel(tc)); + if(pos_error<0) { + errorvel *= -1.0; + } + tc->target_vel = target_vel + errorvel; + } + + //Finally, clip requested velocity at zero + if (tc->target_vel < 0.0) { + tc->target_vel = 0.0; + } + + if (nexttc && nexttc->synchronized) { + //If the next move is synchronized too, then match it's + //requested velocity to the current move + nexttc->target_vel = tc->target_vel; + } +} + + +/** + * Perform parabolic blending if needed between segments and handle status updates. + * This isolates most of the parabolic blend stuff to make the code path + * between tangent and parabolic blends easier to follow. + */ +STATIC int tpDoParabolicBlending(TP_STRUCT * const tp, TC_STRUCT * const tc, + TC_STRUCT * const nexttc) { + + tc_debug_print("in DoParabolicBlend\n"); + tpUpdateBlend(tp,tc,nexttc); + + /* Status updates */ + //Decide which segment we're in depending on which is moving faster + if(tc->currentvel > nexttc->currentvel) { + tpUpdateMovementStatus(tp, tc); + } else { + tpToggleDIOs(nexttc); + tpUpdateMovementStatus(tp, nexttc); + } +#ifdef TP_SHOW_BLENDS + // hack to show blends in axis + tp->motionType = 0; +#endif + + //Update velocity status based on both tc and nexttc + emcmotStatus->current_vel = tc->currentvel + nexttc->currentvel; + + return TP_ERR_OK; +} + + +/** + * Do a complete update on one segment. + * Handles the majority of updates on a single segment for the current cycle. + */ +STATIC int tpUpdateCycle(TP_STRUCT * const tp, + TC_STRUCT * const tc, TC_STRUCT const * const nexttc, int* mode) { + + //placeholders for position for this update + EmcPose before; + + //Store the current position due to this TC + tcGetPos(tc, &before); + + // Update the start velocity if we're not blending yet + if (!tc->blending_next) { + tc->vel_at_blend_start = tc->currentvel; + } + + // Run cycle update with stored cycle time + int res_accel = 1; + double acc=0, vel_desired=0; + int planner_type = GET_TRAJ_PLANNER_TYPE(); + + if(mode == NULL) planner_type = 0; + + if(planner_type != 1){ + // If the slowdown is not too great, use velocity ramping instead of trapezoidal velocity + // Also, don't ramp up for parabolic blends + if (tc->accel_mode && tc->term_cond == TC_TERM_COND_TANGENT) { + if(planner_type == 0) + res_accel = tpCalculateRampAccel(tp, tc, nexttc, &acc, &vel_desired); + } + + // Check the return in case the ramp calculation failed, fall back to trapezoidal + if (res_accel != TP_ERR_OK) { + if(planner_type == 0) + tpCalculateTrapezoidalAccel(tp, tc, nexttc, &acc, &vel_desired); + } + + tcUpdateDistFromAccel(tc, acc, vel_desired, tp->reverse_run); + tpDebugCycleInfo(tp, tc, nexttc, acc); + }else{ + if(*mode == 1){ + double jerk; + double perror; + double req_pos = -1.0; // -1.0 means not provided + tc->cycle_time = tp->cycleTime; + + int is_dec = tpCalculateSCurveAccel(tp, tc, nexttc, &acc, &jerk, &vel_desired, &perror, 1, &req_pos); + if(is_dec == TP_SCURVE_ACCEL_ERROR){ //If the calculation fails, revert to T-shaped acceleration/deceleration. + *mode = TP_SCURVE_ACCEL_ERROR; + res_accel = 1; + acc=0, vel_desired=0; + if (tc->accel_mode && tc->term_cond == TC_TERM_COND_TANGENT) { + res_accel = tpCalculateRampAccel(tp, tc, nexttc, &acc, &vel_desired); + } + // Check the return in case the ramp calculation failed, fall back to trapezoidal + if (res_accel != TP_ERR_OK) { + tpCalculateTrapezoidalAccel(tp, tc, nexttc, &acc, &vel_desired); + } + tcUpdateDistFromAccel(tc, acc, vel_desired, tp->reverse_run); + }else{ + tcUpdateDistFromSCurveAccel(tc, acc, jerk, vel_desired, perror, tp->reverse_run, 0, req_pos); + } + }else{ + double jerk; + double perror; + double req_pos = -1.0; // -1.0 means not provided + int is_dec = tpCalculateSCurveAccel(tp, tc, nexttc, &acc, &jerk, &vel_desired, &perror, 0, &req_pos); + if(is_dec == TP_SCURVE_ACCEL_ERROR){ //If the calculation fails, revert to T-shaped acceleration/deceleration. + *mode = TP_SCURVE_ACCEL_ERROR; + res_accel = 1; + acc=0, vel_desired=0; + if (tc->accel_mode && tc->term_cond == TC_TERM_COND_TANGENT) { + res_accel = tpCalculateRampAccel(tp, tc, nexttc, &acc, &vel_desired); + } + // Check the return in case the ramp calculation failed, fall back to trapezoidal + if (res_accel != TP_ERR_OK) { + tpCalculateTrapezoidalAccel(tp, tc, nexttc, &acc, &vel_desired); + } + tcUpdateDistFromAccel(tc, acc, vel_desired, tp->reverse_run); + }else{ + tcUpdateDistFromSCurveAccel(tc, acc, jerk, vel_desired, perror, tp->reverse_run, is_dec, req_pos); + } + } + tpDebugCycleInfo(tp, tc, nexttc, acc); + + } + + //Check if we're near the end of the cycle and set appropriate changes + tpCheckEndCondition(tp, tc, nexttc); + + EmcPose displacement; + + // Calculate displacement + tcGetPos(tc, &displacement); + emcPoseSelfSub(&displacement, &before); + + //Store displacement (checking for valid pose) + int res_set = tpAddCurrentPos(tp, &displacement); + +#ifdef TC_DEBUG + double mag; + emcPoseMagnitude(&displacement, &mag); + tc_debug_print("cycle movement = %f\n", mag); +#endif + + return res_set; +} + + +/** + * Send default values to status structure. + */ +STATIC int tpUpdateInitialStatus(TP_STRUCT const * const tp) { + // Update queue length + emcmotStatus->tcqlen = tcqLen(&tp->queue); + // Set default value for requested speed + emcmotStatus->requested_vel = 0.0; + //FIXME test if we can do this safely + emcmotStatus->current_vel = 0.0; + return TP_ERR_OK; +} + + +/** + * Flag a segment as needing a split cycle. + * In addition to flagging a segment as splitting, do any preparations to store + * data for the next cycle. + */ +STATIC inline int tcSetSplitCycle(TC_STRUCT * const tc, double split_time, + double v_f) +{ + tp_debug_print("split time for id %d is %.16g\n", tc->id, split_time); + if (tc->splitting != 0 && split_time > 0.0) { + rtapi_print_msg(RTAPI_MSG_ERR,"already splitting on id %d with cycle time %.16g, dx = %.16g, split time %.12g\n", + tc->id, + tc->cycle_time, + tc->target-tc->progress, + split_time); + return TP_ERR_FAIL; + } + tc->splitting = 1; + tc->cycle_time = split_time; + tc->term_vel = v_f; + return 0; +} + + +/** + * Check remaining time in a segment and calculate split cycle if necessary. + * This function estimates how much time we need to complete the next segment. + * If it's greater than one timestep, then we do nothing and carry on. If not, + * then we flag the segment as "splitting", so that during the next cycle, + * it handles the transition to the next segment. + */ +STATIC int tpCheckEndCondition(TP_STRUCT const * const tp, TC_STRUCT * const tc, TC_STRUCT const * const nexttc) { + + //Assume no split time unless we find otherwise + tc->cycle_time = tp->cycleTime; + //Initial guess at dt for next round + double dx = tcGetDistanceToGo(tc, tp->reverse_run); + tc_debug_print("tpCheckEndCondition: dx = %e\n",dx); + + if (dx <= TP_POS_EPSILON) { + //If the segment is close to the target position, then we assume that it's done. + tp_debug_print("close to target, dx = %.12f\n",dx); + //Force progress to land exactly on the target to prevent numerical errors. + tc->progress = tcGetTarget(tc, tp->reverse_run); + + if (!tp->reverse_run) { + tcSetSplitCycle(tc, 0.0, tc->currentvel); + } + if (tc->term_cond == TC_TERM_COND_STOP || tc->term_cond == TC_TERM_COND_EXACT || tp->reverse_run) { + tc->remove = 1; + } + return TP_ERR_OK; + } else if (tp->reverse_run) { + return TP_ERR_NO_ACTION; + } else if (tc->term_cond == TC_TERM_COND_STOP || tc->term_cond == TC_TERM_COND_EXACT) { + return TP_ERR_NO_ACTION; + } + + + double v_f = tpGetRealFinalVel(tp, tc, nexttc); + double v_avg = (tc->currentvel + v_f) / 2.0; + + //Check that we have a non-zero "average" velocity between now and the + //finish. If not, it means that we have to accelerate from a stop, which + //will take longer than the minimum 2 timesteps that each segment takes, so + //we're safely far form the end. + + //Get dt assuming that we can magically reach the final velocity at + //the end of the move. + // + //KLUDGE: start with a value below the cutoff + double dt = TP_TIME_EPSILON / 2.0; + if (v_avg > TP_VEL_EPSILON) { + //Get dt from distance and velocity (avoid div by zero) + dt = fmax(dt, dx / v_avg); + } else { + if ( dx > (v_avg * tp->cycleTime) && dx > TP_POS_EPSILON) { + tc_debug_print(" below velocity threshold, assuming far from end\n"); + return TP_ERR_NO_ACTION; + } + } + + //Calculate the acceleration this would take: + + double dv = v_f - tc->currentvel; + double a_f = dv / dt; + + //If this is a valid acceleration, then we're done. If not, then we solve + //for v_f and dt given the max acceleration allowed. + double a_max = tcGetTangentialMaxAccel(tc); + + //If we exceed the maximum acceleration, then the dt estimate is too small. + double a = a_f; + int recalc = sat_inplace(&a, a_max); + + //Need to recalculate vf and above + if (recalc) { + tc_debug_print(" recalculating with a_f = %f, a = %f\n", a_f, a); + double disc = pmSq(tc->currentvel / a) + 2.0 / a * dx; + if (disc < 0) { + //Should mean that dx is too big, i.e. we're not close enough + tc_debug_print(" dx = %f, too large, not at end yet\n",dx); + return TP_ERR_NO_ACTION; + } + + if (disc < TP_TIME_EPSILON * TP_TIME_EPSILON) { + tc_debug_print("disc too small, skipping sqrt\n"); + dt = -tc->currentvel / a; + } else if (a > 0) { + tc_debug_print("using positive sqrt\n"); + dt = -tc->currentvel / a + pmSqrt(disc); + } else { + tc_debug_print("using negative sqrt\n"); + dt = -tc->currentvel / a - pmSqrt(disc); + } + + tc_debug_print(" revised dt = %f\n", dt); + //Update final velocity with actual result + v_f = tc->currentvel + dt * a; + } + + if (dt < TP_TIME_EPSILON) { + //Close enough, call it done + tc_debug_print("revised dt small, finishing tc\n"); + tc->progress = tcGetTarget(tc, tp->reverse_run); + tcSetSplitCycle(tc, 0.0, v_f); + } else if (dt < tp->cycleTime ) { + tc_debug_print(" corrected v_f = %f, a = %f\n", v_f, a); + tcSetSplitCycle(tc, dt, v_f); + } else { + tc_debug_print(" dt = %f, not at end yet\n",dt); + return TP_ERR_NO_ACTION; + } + return TP_ERR_OK; +} + + +STATIC int tpHandleSplitCycle(TP_STRUCT * const tp, TC_STRUCT * const tc, + TC_STRUCT * const nexttc) +{ + if (tc->remove) { + //Don't need to update since this segment is flagged for removal + return TP_ERR_NO_ACTION; + } + + //Pose data to calculate movement due to finishing current TC + EmcPose before; + tcGetPos(tc, &before); + + tp_debug_print("tc id %d splitting\n",tc->id); + //Shortcut tc update by assuming we arrive at end + tc->progress = tcGetTarget(tc,tp->reverse_run); + //Get displacement from prev. position + EmcPose displacement; + tcGetPos(tc, &displacement); + emcPoseSelfSub(&displacement, &before); + + // Update tp's position (checking for valid pose) + tpAddCurrentPos(tp, &displacement); + +#ifdef TC_DEBUG + double mag; + emcPoseMagnitude(&displacement, &mag); + tc_debug_print("cycle movement = %f\n",mag); +#endif + + // Trigger removal of current segment at the end of the cycle + tc->remove = 1; + + if (!nexttc) { + tp_debug_print("no nexttc in split cycle\n"); + return TP_ERR_OK; + } + + switch (tc->term_cond) { + case TC_TERM_COND_TANGENT: + nexttc->cycle_time = tp->cycleTime - tc->cycle_time; + // In S-curve mode, use actual current velocity instead of expected term_vel + // S-curve can't change velocity instantly, term_vel is just desired value + if (GET_TRAJ_PLANNER_TYPE() == 1) { + nexttc->currentvel = tc->currentvel; + // Inherit acceleration, but limit to nexttc's allowed range + // Important for line-to-arc transitions where arc has lower tangential accel + double maxacc_next = tcGetTangentialMaxAccel(nexttc); + nexttc->currentacc = saturate(tc->currentacc, maxacc_next); + nexttc->currentjerk = tc->currentjerk; + tp_debug_print("Doing tangent split (S-curve): vel=%f, acc=%f (limited by %f), jerk=%f\n", + nexttc->currentvel, nexttc->currentacc, maxacc_next, nexttc->currentjerk); + } else { + // Trapezoidal: can use term_vel (assumes instant velocity change) + nexttc->currentvel = tc->term_vel; + tp_debug_print("Doing tangent split (trapezoidal): vel=%f\n", nexttc->currentvel); + } + break; + case TC_TERM_COND_PARABOLIC: + break; + case TC_TERM_COND_STOP: + break; + case TC_TERM_COND_EXACT: + break; + default: + rtapi_print_msg(RTAPI_MSG_ERR,"unknown term cond %d in segment %d\n", + tc->term_cond, + tc->id); + } + + // Run split cycle update with remaining time in nexttc + // KLUDGE: use next cycle after nextc to prevent velocity dip (functions fail gracefully w/ NULL) + int queue_dir_step = tp->reverse_run ? -1 : 1; + TC_STRUCT *next2tc = tcqItem(&tp->queue, queue_dir_step*2); + + int mode = 0; + tpUpdateCycle(tp, nexttc, next2tc, &mode); + + // Update status for the split portion + // FIXME redundant tangent check, refactor to switch + if (tc->cycle_time > nexttc->cycle_time && tc->term_cond == TC_TERM_COND_TANGENT) { + //Majority of time spent in current segment + tpToggleDIOs(tc); + tpUpdateMovementStatus(tp, tc); + } else { + tpToggleDIOs(nexttc); + } + tpUpdateMovementStatus(tp, nexttc); + + return TP_ERR_OK; +} + +STATIC int tpHandleRegularCycle(TP_STRUCT * const tp, + TC_STRUCT * const tc, + TC_STRUCT * const nexttc) +{ + if (tc->remove) { + //Don't need to update since this segment is flagged for removal + return TP_ERR_NO_ACTION; + } + //Run with full cycle time + tc_debug_print("Normal cycle\n"); + tc->cycle_time = tp->cycleTime; + + int mode = 0; + tpUpdateCycle(tp, tc, nexttc, &mode); + + /* Parabolic blending */ + + double v_this = 0.0, v_next = 0.0; + + // cap the blend velocity at the current requested speed (factoring in feed override) + double target_vel_this = tpGetRealTargetVel(tp, tc); + double target_vel_next = tpGetRealTargetVel(tp, nexttc); + + if(mode != TP_SCURVE_ACCEL_ERROR && GET_TRAJ_PLANNER_TYPE() == 1) + tpComputeBlendSCurveVelocity(tc, nexttc, target_vel_this, target_vel_next, &v_this, &v_next, NULL); + else + tpComputeBlendVelocity(tc, nexttc, target_vel_this, target_vel_next, &v_this, &v_next, NULL); + tc->blend_vel = v_this; + if (nexttc) { + nexttc->blend_vel = v_next; + } + + if (nexttc && tcIsBlending(tc)) { + tpDoParabolicBlending(tp, tc, nexttc); + } else { + //Update status for a normal step + tpToggleDIOs(tc); + tpUpdateMovementStatus(tp, tc); + } + return TP_ERR_OK; +} + + +/** + * Calculate an updated goal position for the next timestep. + * This is the brains of the operation. It's called every TRAJ period and is + * expected to set tp->currentPos to the new machine position. Lots of other + * const tp fields (depth, done, etc) have to be twiddled to communicate the + * status; I think those are spelled out here correctly and I can't clean it up + * without breaking the API that the TP presents to motion. + */ +int tpRunCycle(TP_STRUCT * const tp, long period) +{ + (void)period; + //Pointers to current and next trajectory component + TC_STRUCT *tc; + TC_STRUCT *nexttc; + + /* Get pointers to current and relevant future segments. It's ok here if + * future segments don't exist (NULL pointers) as we check for this later). + */ + + int queue_dir_step = tp->reverse_run ? -1 : 1; + tc = tcqItem(&tp->queue, 0); + nexttc = tcqItem(&tp->queue, queue_dir_step * 1); + + //Set GUI status to "zero" state + tpUpdateInitialStatus(tp); + +#ifdef TC_DEBUG + //Hack debug output for timesteps + // NOTE: need to track every timestep, even those where the trajectory planner is idle + static double time_elapsed = 0; + time_elapsed+=tp->cycleTime; +#endif + + //If we have a NULL pointer, then the queue must be empty, so we're done. + if(!tc) { + tpHandleEmptyQueue(tp); + return TP_ERR_WAITING; + } + + tc_debug_print("-------------------\n"); + + + /* If the queue empties enough, assume that the program is near the end. + * This forces the last segment to be "finalized" to let the optimizer run.*/ + /*tpHandleLowQueue(tp);*/ + + /* If we're aborting or pausing and the velocity has reached zero, then we + * don't need additional planning and can abort here. */ + if (tpHandleAbort(tp, tc, nexttc) == TP_ERR_STOPPED) { + return TP_ERR_STOPPED; + } + + //Return early if we have a reason to wait (i.e. not ready for motion) + if (tpCheckAtSpeed(tp, tc) != TP_ERR_OK){ + return TP_ERR_WAITING; + } + + int res_activate = tpActivateSegment(tp, tc); + if (res_activate != TP_ERR_OK ) { + return res_activate; + } + + // Preprocess rigid tap move (handles threading direction reversals) + if (tc->motion_type == TC_RIGIDTAP) { + tpUpdateRigidTapState(tp, tc); + } + + /** If synchronized with spindle, calculate requested velocity to track + * spindle motion.*/ + switch (tc->synchronized) { + case TC_SYNC_NONE: + emcmotStatus->spindleSync = 0; + break; + case TC_SYNC_VELOCITY: + tp_debug_print("sync velocity\n"); + tpSyncVelocityMode(tp, tc, nexttc); + break; + case TC_SYNC_POSITION: + tp_debug_print("sync position\n"); + tpSyncPositionMode(tp, tc, nexttc); + break; + default: + tp_debug_print("unrecognized spindle sync state!\n"); + break; + } + +#ifdef TC_DEBUG + EmcPose pos_before = tp->currentPos; +#endif + + + tcClearFlags(tc); + tcClearFlags(nexttc); + // Update the current tc + if (tc->splitting) { + tpHandleSplitCycle(tp, tc, nexttc); + } else { + tpHandleRegularCycle(tp, tc, nexttc); + } + +#ifdef TC_DEBUG + double mag; + EmcPose disp; + emcPoseSub(&tp->currentPos, &pos_before, &disp); + emcPoseMagnitude(&disp, &mag); + tc_debug_print("time: %.12e total movement = %.12e vel = %.12e\n", + time_elapsed, + mag, emcmotStatus->current_vel); + + tc_debug_print("tp_displacement = %.12e %.12e %.12e time = %.12e\n", + disp.tran.x, + disp.tran.y, + disp.tran.z, + time_elapsed); +#endif + + // If TC is complete, remove it from the queue. + if (tc->remove) { + tpCompleteSegment(tp, tc); + } + + return TP_ERR_OK; +} + +int tpSetSpindleSync(TP_STRUCT * const tp, int spindle, double sync, int mode) { + if(sync) { + if (mode) { + tp->synchronized = TC_SYNC_VELOCITY; + } else { + tp->synchronized = TC_SYNC_POSITION; + } + tp->uu_per_rev = sync; + tp->spindle.spindle_num = spindle; + } else + tp->synchronized = 0; + + return TP_ERR_OK; +} + +int tpPause(TP_STRUCT * const tp) +{ + if (0 == tp) { + return TP_ERR_FAIL; + } + tp->pausing = 1; + return TP_ERR_OK; +} + +int tpResume(TP_STRUCT * const tp) +{ + if (0 == tp) { + return TP_ERR_FAIL; + } + tp->pausing = 0; + return TP_ERR_OK; +} + +int tpAbort(TP_STRUCT * const tp) +{ + if (0 == tp) { + return TP_ERR_FAIL; + } + + if (!tp->aborting) { + /* const to abort, signal a pause and set our abort flag */ + tpPause(tp); + tp->aborting = 1; + } + return tpClearDIOs(tp); //clears out any already cached DIOs +} + +int tpGetMotionType(TP_STRUCT * const tp) +{ + return tp->motionType; +} + +int tpGetPos(TP_STRUCT const * const tp, EmcPose * const pos) +{ + + if (0 == tp) { + ZERO_EMC_POSE((*pos)); + return TP_ERR_FAIL; + } else { + *pos = tp->currentPos; + } + + return TP_ERR_OK; +} + +int tpIsDone(TP_STRUCT * const tp) +{ + if (0 == tp) { + return TP_ERR_OK; + } + + return tp->done; +} + +int tpQueueDepth(TP_STRUCT * const tp) +{ + if (0 == tp) { + return TP_ERR_OK; + } + + return tp->depth; +} + +int tpActiveDepth(TP_STRUCT * const tp) +{ + if (0 == tp) { + return TP_ERR_OK; + } + + return tp->activeDepth; +} + +int tpSetAout(TP_STRUCT * const tp, unsigned char index, double start, double end) { + (void)end; + if (0 == tp) { + return TP_ERR_FAIL; + } + tp->syncdio.anychanged = 1; //something has changed + tp->syncdio.aio_mask |= (1 << index); + tp->syncdio.aios[index] = start; + return TP_ERR_OK; +} + +int tpSetDout(TP_STRUCT * const tp, int index, unsigned char start, unsigned char end) { + (void)end; + if (0 == tp) { + return TP_ERR_FAIL; + } + tp->syncdio.anychanged = 1; //something has changed + tp->syncdio.dio_mask |= (1 << index); + if (start > 0) + tp->syncdio.dios[index] = 1; // the end value can't be set from canon currently, and has the same value as start + else + tp->syncdio.dios[index] = -1; + return TP_ERR_OK; +} + +int tpSetRunDir(TP_STRUCT * const tp, tc_direction_t dir) +{ + // Can't change direction while moving + if (tpIsMoving(tp)) { + return TP_ERR_FAIL; + } + + switch (dir) { + case TC_DIR_FORWARD: + case TC_DIR_REVERSE: + tp->reverse_run = dir; + return TP_ERR_OK; + default: + rtapi_print_msg(RTAPI_MSG_ERR,"Invalid direction flag in SetRunDir"); + return TP_ERR_FAIL; + } +} + +int tpIsMoving(TP_STRUCT const * const tp) +{ + + //TODO may be better to explicitly check velocities on the first 2 segments, but this is messy + if (emcmotStatus->current_vel >= TP_VEL_EPSILON ) { + tp_debug_print("TP moving, current_vel = %.16g\n", emcmotStatus->current_vel); + return true; + } else if (tp->spindle.waiting_for_index != MOTION_INVALID_ID || tp->spindle.waiting_for_atspeed != MOTION_INVALID_ID) { + tp_debug_print("TP moving, waiting for index or atspeed\n"); + return true; + } + return false; +} + +// api: functions called by motion: +EXPORT_SYMBOL(tpMotFunctions); +EXPORT_SYMBOL(tpMotData); + +EXPORT_SYMBOL(tpAbort); +EXPORT_SYMBOL(tpActiveDepth); +EXPORT_SYMBOL(tpAddCircle); +EXPORT_SYMBOL(tpAddLine); +EXPORT_SYMBOL(tpAddRigidTap); +EXPORT_SYMBOL(tpClear); +EXPORT_SYMBOL(tpCreate); +EXPORT_SYMBOL(tpGetExecId); +EXPORT_SYMBOL(tpGetExecTag); +EXPORT_SYMBOL(tpGetMotionType); +EXPORT_SYMBOL(tpGetPos); +EXPORT_SYMBOL(tpIsDone); +EXPORT_SYMBOL(tpPause); +EXPORT_SYMBOL(tpQueueDepth); +EXPORT_SYMBOL(tpResume); +EXPORT_SYMBOL(tpRunCycle); +EXPORT_SYMBOL(tpSetAmax); +EXPORT_SYMBOL(tpSetAout); +EXPORT_SYMBOL(tpSetCycleTime); +EXPORT_SYMBOL(tpSetDout); +EXPORT_SYMBOL(tpSetId); +EXPORT_SYMBOL(tpSetPos); +EXPORT_SYMBOL(tpSetRunDir); +EXPORT_SYMBOL(tpSetSpindleSync); +EXPORT_SYMBOL(tpSetTermCond); +EXPORT_SYMBOL(tpSetVlimit); +EXPORT_SYMBOL(tpSetVmax); + +EXPORT_SYMBOL(tcqFull); + +#undef MAKE_TP_HAL_PINS diff --git a/wasm-port/vendor/linuxcnc/src/emc/tp/tp.h b/wasm-port/vendor/linuxcnc/src/emc/tp/tp.h new file mode 100644 index 0000000..e00b457 --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/tp/tp.h @@ -0,0 +1,99 @@ +/******************************************************************** +* Description: tp.h +* Trajectory planner based on TC elements +* +* Derived from a work by Fred Proctor & Will Shackleford +* +* Author: +* License: GPL Version 2 +* System: Linux +* +* Copyright (c) 2004 All rights reserved. +* +********************************************************************/ +#ifndef TP_H +#define TP_H + +#include + +#include "tc_types.h" +#include "tp_types.h" +#include "tcq.h" + +// functions not used by motmod: +int tpAddCurrentPos(TP_STRUCT * const tp, EmcPose const * const disp); +int tpSetCurrentPos(TP_STRUCT * const tp, EmcPose const * const pos); +void tpToggleDIOs(TC_STRUCT * const tc); //gets called when a new tc is + //taken from the queue. it checks + //and toggles all needed DIO's +int tpIsMoving(TP_STRUCT const * const tp); +int tpInit(TP_STRUCT * const tp); + +// functions used by motmod: +int tpCreate(TP_STRUCT * const tp, int _queueSize,int id); +int tpClear(TP_STRUCT * const tp); +int tpClearDIOs(TP_STRUCT * const tp); +int tpSetCycleTime(TP_STRUCT * tp, double secs); +int tpSetVmax(TP_STRUCT * tp, double vmax, double ini_maxvel); +int tpSetVlimit(TP_STRUCT * tp, double limit); +int tpSetAmax(TP_STRUCT * tp, double amax); +int tpSetId(TP_STRUCT * tp, int id); +int tpGetExecId(TP_STRUCT * tp); +struct state_tag_t tpGetExecTag(TP_STRUCT * const tp); +int tpSetTermCond(TP_STRUCT * tp, int cond, double tolerance); +int tpSetPos(TP_STRUCT * tp, EmcPose const * const pos); +int tpRunCycle(TP_STRUCT * tp, long period); +int tpPause(TP_STRUCT * tp); +int tpResume(TP_STRUCT * tp); +int tpAbort(TP_STRUCT * tp); +int tpAddRigidTap(TP_STRUCT * const tp, + EmcPose end, + double vel, + double ini_maxvel, + double acc, + double ini_maxjerk, + unsigned char enables, + double scale, + struct state_tag_t tag); +int tpAddLine(TP_STRUCT * const tp, EmcPose end, int canon_motion_type, + double vel, double ini_maxvel, double acc, double ini_maxjerk, unsigned char enables, + char atspeed, int indexrotary, struct state_tag_t tag); +int tpAddCircle(TP_STRUCT * const tp, EmcPose end, PmCartesian center, + PmCartesian normal, int turn, int canon_motion_type, double vel, + double ini_maxvel, double acc, double ini_maxjerk, unsigned char enables, + char atspeed, struct state_tag_t tag); +int tpGetPos(TP_STRUCT const * const tp, EmcPose * const pos); +int tpIsDone(TP_STRUCT * const tp); +int tpQueueDepth(TP_STRUCT * const tp); +int tpActiveDepth(TP_STRUCT * const tp); +int tpGetMotionType(TP_STRUCT * const tp); +int tpSetSpindleSync(TP_STRUCT * const tp, int spindle, double sync, int wait); + +int tpSetAout(TP_STRUCT * const tp, unsigned char index, double start, double end); +int tpSetDout(TP_STRUCT * const tp, int index, unsigned char start, unsigned char end); //gets called to place DIO toggles on the TC queue + +int tpSetRunDir(TP_STRUCT * const tp, tc_direction_t dir); + +//--------------------------------------------------------------------- +// Module interface +void tpMotFunctions(void(*pDioWrite)(int,char) + ,void(*pAioWrite)(int,double) + ,void(*pSetRotaryUnlock)(int,int) + ,int( *pGetRotaryUnlock)(int) + ,double(*paxis_get_vel_limit)(int) + ,double(*paxis_get_acc_limit)(int) + ); + +// These are here so we don't need to include "motion/motion.h" +// because that feels very wrong. The real solution is to untangle +// motion controller and trajectory planner sources. Only the shared +// data should be exposed to each other. +typedef struct emcmot_status_t emcmot_status_t; +typedef struct emcmot_config_t emcmot_config_t; + +void tpMotData(emcmot_status_t * + ,emcmot_config_t * + ); +//--------------------------------------------------------------------- + +#endif /* TP_H */ diff --git a/wasm-port/vendor/linuxcnc/src/emc/tp/tp_debug.h b/wasm-port/vendor/linuxcnc/src/emc/tp/tp_debug.h new file mode 100644 index 0000000..cf9dce4 --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/tp/tp_debug.h @@ -0,0 +1,65 @@ +/******************************************************************** +* Description: tc_debug.h +* +* +* Author: Robert W. Ellenberg +* License: GPL Version 2 +* System: Linux +* +* Copyright (c) 2013 All rights reserved. +* +* Last change: +********************************************************************/ +#ifndef TP_DEBUG_H +#define TP_DEBUG_H + +#include /* printing functions */ + +/** TP debug stuff */ +#ifdef TP_DEBUG +//Kludge because I didn't know any better at the time +//FIXME replace these with better names? +#define tp_debug_print(...) rtapi_print(__VA_ARGS__) +#elif defined(UNIT_TEST) +#include +#define tp_debug_print(...) printf(__VA_ARGS__) +#else +#define tp_debug_print(...) +#endif + +// Verbose but effective wrappers for building faux-JSON debug output for a function +#define tp_debug_json_double(varname_) tp_debug_print("%s: %g, ", #varname_, varname_) +#define tp_debug_json_start(fname_) tp_debug_print("%s: {", #fname_) +#define tp_debug_json_end() tp_debug_print("}\n") + +/** Use for profiling to make static function names visible */ +#ifdef TP_PROFILE +#define STATIC +#else +#define STATIC static +#endif + +/** "TC" debug info for inspecting trajectory planner output at each timestep */ +#ifdef TC_DEBUG +#define tc_debug_print(...) rtapi_print(__VA_ARGS__) +#else +#define tc_debug_print(...) +#endif + +/** TP position data output to debug acceleration spikes */ +#ifdef TP_POSEMATH_DEBUG +#define tp_posemath_debug(...) rtapi_print(__VA_ARGS__) +#else +#define tp_posemath_debug(...) +#endif + +/** TP misc data logging */ +#ifdef TP_INFO_LOGGING +#define tp_info_print(...) rtapi_print(__VA_ARGS__) +#else +#define tp_info_print(...) +#endif + +int gdb_fake_catch(int condition); +int gdb_fake_assert(int condition); +#endif diff --git a/wasm-port/vendor/linuxcnc/src/emc/tp/tp_types.h b/wasm-port/vendor/linuxcnc/src/emc/tp/tp_types.h new file mode 100644 index 0000000..6687ef3 --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/tp/tp_types.h @@ -0,0 +1,154 @@ +/******************************************************************** +* Description: tp_types.h +* Trajectory planner types and constants +* +* Derived from a work by Fred Proctor & Will Shackleford +* +* Author: +* License: GPL Version 2 +* System: Linux +* +* Copyright (c) 2004 All rights reserved. +* +********************************************************************/ +#ifndef TP_TYPES_H +#define TP_TYPES_H + +#include +#include + +#include "tc_types.h" +#include "tcq.h" + +#define TP_DEFAULT_QUEUE_SIZE 32 +/* Minimum length of a segment in cycles (must be greater than 1 to ensure each + * segment is hit at least once.) */ +#define TP_MIN_SEGMENT_CYCLES 1.02 +/* Values chosen for accel ratio to match parabolic blend acceleration + * limits. */ +#define TP_OPTIMIZATION_CUTOFF 4 +/* If the queue is shorter than the threshold, assume that we're approaching + * the end of the program */ +#define TP_QUEUE_THRESHOLD 3 + +/* closeness to zero, for determining if a move is pure rotation */ +#define TP_PURE_ROTATION_EPSILON 1e-6 + +/* "neighborhood" size (if two values differ by less than the epsilon, + * then they are effectively equal.)*/ +#define TP_ACCEL_EPSILON 1e-4 +#define TP_VEL_EPSILON 1e-8 +#define TP_POS_EPSILON 1e-12 +#define TP_TIME_EPSILON 1e-12 +#define TP_ANGLE_EPSILON 1e-6 +#define TP_ANGLE_EPSILON_SQ (TP_ANGLE_EPSILON * TP_ANGLE_EPSILON) +#define TP_MIN_ARC_ANGLE 1e-3 +#define TP_MIN_ARC_LENGTH 1e-6 +#define TP_BIG_NUM 1e10 + +/** + * TP return codes. + * This enum is a catch-all for useful return statuses from TP + * internal functions. This may be replaced with a better system in + * the future. + */ +typedef enum { + TP_ERR_INVALID = -9, + TP_ERR_INPUT_TYPE = -8, + TP_ERR_TOLERANCE = -7, + TP_ERR_RADIUS_TOO_SMALL = -6, + TP_ERR_GEOM = -5, + TP_ERR_RANGE = -4, + TP_ERR_MISSING_OUTPUT = -3, + TP_ERR_MISSING_INPUT = -2, + TP_ERR_FAIL = -1, + TP_ERR_OK = 0, + TP_ERR_NO_ACTION, + TP_ERR_SLOWING, + TP_ERR_STOPPED, + TP_ERR_WAITING, + TP_ERR_ZERO_LENGTH, + TP_ERR_REVERSE_EMPTY, + TP_ERR_LAST +} tp_err_t; + +/** + * Persistent data for spindle status within tpRunCycle. + * This structure encapsulates some static variables to simplify refactoring of + * synchronized motion code. + */ +typedef struct { + int spindle_num; + double offset; + double revs; + int waiting_for_index; + int waiting_for_atspeed; +} tp_spindle_t; + +/** + * Trajectory planner state structure. + * Stores persistent data for the trajectory planner that should be accessible + * by outside functions. + */ +typedef struct { + TC_QUEUE_STRUCT queue; + tp_spindle_t spindle; //Spindle data + + EmcPose currentPos; + EmcPose goalPos; + + int queueSize; + double cycleTime; + + double vMax; /* vel for subsequent moves */ + double ini_maxvel; /* max velocity allowed by machine + constraints (INI file) for + subsequent moves */ + double vLimit; /* absolute upper limit on all vels */ + + double aMax; /* max accel (unused) */ + double ini_maxjerk; + //FIXME this shouldn't be a separate limit, + double aMaxCartesian; /* max cartesian acceleration by machine bounds */ + double aLimit; /* max accel (unused) */ + + double wMax; /* rotational velocity max */ + double wDotMax; /* rotational acceleration max */ + int nextId; + int execId; + struct state_tag_t execTag; /* state tag corresponding to running motion */ + int termCond; + int done; + int depth; /* number of total queued motions */ + int activeDepth; /* number of motions blending */ + int aborting; + int pausing; + int reverse_run; /* Indicates that TP is running in reverse */ + int motionType; + double tolerance; /* for subsequent motions, stay within this + distance of the programmed path during + blends */ + int synchronized; // spindle sync required for this move + int velocity_mode; /* TRUE if spindle sync is in velocity mode, + FALSE if in position mode */ + double uu_per_rev; /* user units per spindle revolution */ + + + syncdio_t syncdio; //record tpSetDout's here + +} TP_STRUCT; + + +/** + * Describes blend modes used in the trajectory planner. + * @note these values are used as array indices, so make sure valid options + * start at 0 and increase by one. + */ +typedef enum { + NO_BLEND = -1, + PARABOLIC_BLEND, + TANGENT_SEGMENTS_BLEND, + ARC_BLEND +} tc_blend_type_t; + +#endif /* TP_TYPES_H */ diff --git a/wasm-port/vendor/linuxcnc/src/libnml/posemath/_posemath.c b/wasm-port/vendor/linuxcnc/src/libnml/posemath/_posemath.c new file mode 100644 index 0000000..241a7d5 --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/libnml/posemath/_posemath.c @@ -0,0 +1,2005 @@ +/******************************************************************** +* Description: _posemath.c +* C definitions for pose math library data types and manipulation +* functions. +* +* Derived from a work by Fred Proctor & Will Shackleford +* +* Author: +* License: LGPL Version 2 +* System: Linux +* +* Copyright (c) 2004 All rights reserved. +* +* Last change: +********************************************************************/ + +#if defined(PM_PRINT_ERROR) && defined(rtai) +#undef PM_PRINT_ERROR +#endif + +#if defined(PM_DEBUG) && defined(rtai) +#undef PM_DEBUG +#endif + +#ifdef PM_PRINT_ERROR +#define PM_DEBUG /* have to have debug with printing */ +#include +#include +#endif +#include "posemath.h" + +#include +#include + +#include "sincos.h" + +/* global error number */ +int pmErrno = PM_OK; + +#ifdef PM_PRINT_ERROR + +void pmPrintError(const char *fmt, ...) +{ + va_list args; + + va_start(args, fmt); + vfprintf(stderr, fmt, args); + va_end(args); +} + +/* error printing function */ +void pmPerror(const char *s) +{ + char *pmErrnoString; + + switch (pmErrno) { + case 0: + /* no error */ + return; + + case PM_ERR: + pmErrnoString = "unspecified error"; + break; + + case PM_IMPL_ERR: + pmErrnoString = "not implemented"; + break; + + case PM_NORM_ERR: + pmErrnoString = "expected normalized value"; + break; + + case PM_DIV_ERR: + pmErrnoString = "divide by zero"; + break; + + default: + pmErrnoString = "unassigned error"; + break; + } + + if (s != 0 && s[0] != 0) { + fprintf(stderr, "%s: %s\n", s, pmErrnoString); + } else { + fprintf(stderr, "%s\n", pmErrnoString); + } +} + +#endif /* PM_PRINT_ERROR */ + +/* fuzz checker */ +#define IS_FUZZ(a,fuzz) (fabs(a) < (fuzz)) + +/* Pose Math Basis Functions */ + +int pmClose(double a, double b, double eps) { return ((fabs((a) - (b)) < (eps)) ? 1 : 0); } + +/* Scalar functions */ + +double pmSqrt(double x) +{ + if (x > 0.0) { + pmErrno = PM_OK; + return sqrt(x); + } + + if (x > SQRT_FUZZ) { + pmErrno = PM_OK; + return 0.0; + } +#ifdef PM_PRINT_ERROR + pmPrintError("sqrt of large negative number\n"); +#endif + + pmErrno = PM_ERR; + return 0.0; +} + +/* Translation rep conversion functions */ + +int pmCartSphConvert(PmCartesian const * const v, PmSpherical * const s) +{ + double _r; + + s->theta = atan2(v->y, v->x); + s->r = pmSqrt(pmSq(v->x) + pmSq(v->y) + pmSq(v->z)); + _r = pmSqrt(pmSq(v->x) + pmSq(v->y)); + s->phi = atan2(_r, v->z); + + return pmErrno = PM_OK; +} + +int pmCartCylConvert(PmCartesian const * const v, PmCylindrical * const c) +{ + c->theta = atan2(v->y, v->x); + c->r = pmSqrt(pmSq(v->x) + pmSq(v->y)); + c->z = v->z; + + return pmErrno = PM_OK; +} + +int pmSphCartConvert(PmSpherical const * const s, PmCartesian * const v) +{ + double _r; + + _r = s->r * sin(s->phi); + v->z = s->r * cos(s->phi); + v->x = _r * cos(s->theta); + v->y = _r * sin(s->theta); + + return pmErrno = PM_OK; +} + +int pmSphCylConvert(PmSpherical const * const s, PmCylindrical * const c) +{ + c->theta = s->theta; + c->r = s->r * cos(s->phi); + c->z = s->r * sin(s->phi); + + return pmErrno = PM_OK; +} + +int pmCylCartConvert(PmCylindrical const * const c, PmCartesian * const v) +{ + v->x = c->r * cos(c->theta); + v->y = c->r * sin(c->theta); + v->z = c->z; + + return pmErrno = PM_OK; +} + +int pmCylSphConvert(PmCylindrical const * const c, PmSpherical * const s) +{ + s->theta = c->theta; + s->r = pmSqrt(pmSq(c->r) + pmSq(c->z)); + s->phi = atan2(c->z, c->r); + + return pmErrno = PM_OK; +} + +/* Rotation rep conversion functions */ + +int pmAxisAngleQuatConvert(PmAxis axis, double a, PmQuaternion * const q) +{ + double sh; + + a *= 0.5; + pm_sincos(a, &sh, &(q->s)); + + switch (axis) { + case PM_X: + q->x = sh; + q->y = 0.0; + q->z = 0.0; + break; + + case PM_Y: + q->x = 0.0; + q->y = sh; + q->z = 0.0; + break; + + case PM_Z: + q->x = 0.0; + q->y = 0.0; + q->z = sh; + break; + + default: +#ifdef PM_PRINT_ERROR + pmPrintError("error: bad axis in pmAxisAngleQuatConvert\n"); +#endif + return pmErrno = PM_ERR; + } + + if (q->s < 0.0) { + q->s *= -1.0; + q->x *= -1.0; + q->y *= -1.0; + q->z *= -1.0; + } + + return pmErrno = PM_OK; +} + +int pmRotQuatConvert(PmRotationVector const * const r, PmQuaternion * const q) +{ + double sh; + +#ifdef PM_DEBUG + /* make sure r is normalized */ + //FIXME breaks const promise + PmRotationVector r_raw = *r; + if (0 != pmRotNorm(&r_raw, r)) { +#ifdef PM_PRINT_ERROR + pmPrintError + ("error: pmRotQuatConvert rotation vector not normalized\n"); +#endif + return pmErrno = PM_NORM_ERR; + } +#endif + + if (pmClose(r->s, 0.0, QS_FUZZ)) { + q->s = 1.0; + q->x = q->y = q->z = 0.0; + + return pmErrno = PM_OK; + } + + pm_sincos(r->s / 2.0, &sh, &(q->s)); + + if (q->s >= 0.0) { + q->x = r->x * sh; + q->y = r->y * sh; + q->z = r->z * sh; + } else { + q->s *= -1; + q->x = -r->x * sh; + q->y = -r->y * sh; + q->z = -r->z * sh; + } + + return pmErrno = PM_OK; +} + +int pmRotMatConvert(PmRotationVector const * const r, PmRotationMatrix * const m) +{ + double s, c, omc; + +#ifdef PM_DEBUG + if (!pmRotIsNorm(r)) { +#ifdef PM_PRINT_ERROR + pmPrintError("Bad vector in pmRotMatConvert\n"); +#endif + return pmErrno = PM_NORM_ERR; + } +#endif + + pm_sincos(r->s, &s, &c); + + /* from space book */ + m->x.x = c + pmSq(r->x) * (omc = 1 - c); /* omc = One Minus Cos */ + m->y.x = -r->z * s + r->x * r->y * omc; + m->z.x = r->y * s + r->x * r->z * omc; + + m->x.y = r->z * s + r->y * r->x * omc; + m->y.y = c + pmSq(r->y) * omc; + m->z.y = -r->x * s + r->y * r->z * omc; + + m->x.z = -r->y * s + r->z * r->x * omc; + m->y.z = r->x * s + r->z * r->y * omc; + m->z.z = c + pmSq(r->z) * omc; + + return pmErrno = PM_OK; +} + +int pmRotZyzConvert(PmRotationVector const * const r, PmEulerZyz * const zyz) +{ + (void)r; + (void)zyz; +#ifdef PM_DEBUG +#ifdef PM_PRINT_ERROR + pmPrintError("error: pmRotZyzConvert not implemented\n"); +#endif + return pmErrno = PM_IMPL_ERR; +#else + return PM_IMPL_ERR; +#endif +} + +int pmRotZyxConvert(PmRotationVector const * const r, PmEulerZyx * const zyx) +{ + PmRotationMatrix m; + int r1, r2; + + r1 = pmRotMatConvert(r, &m); + r2 = pmMatZyxConvert(&m, zyx); + + return pmErrno = (r1 || r2) ? PM_NORM_ERR : PM_OK; +} + +int pmRotRpyConvert(PmRotationVector const * const r, PmRpy * const rpy) +{ + PmQuaternion q; + int r1, r2; + + q.s = q.x = q.y = q.z = 0.0; + + r1 = pmRotQuatConvert(r, &q); + r2 = pmQuatRpyConvert(&q, rpy); + + return (r1 || r2) ? pmErrno : PM_OK; +} + +int pmQuatRotConvert(PmQuaternion const * const q, PmRotationVector * const r) +{ + double sh; + +#ifdef PM_DEBUG + if (!pmQuatIsNorm(q)) { +#ifdef PM_PRINT_ERROR + pmPrintError("Bad quaternion in pmQuatRotConvert\n"); +#endif + return pmErrno = PM_NORM_ERR; + } +#endif + if (r == 0) { +#ifdef PM_PRINT_ERROR + pmPrintError("pmQuatRotConvert: null pointer passed as rotation vector\n"); +#endif + return pmErrno = PM_ERR; + } + + sh = pmSqrt(pmSq(q->x) + pmSq(q->y) + pmSq(q->z)); + + if (sh > QSIN_FUZZ) { + r->s = 2.0 * atan2(sh, q->s); + r->x = q->x / sh; + r->y = q->y / sh; + r->z = q->z / sh; + } else { + r->s = 0.0; + r->x = 0.0; + r->y = 0.0; + r->z = 0.0; + } + + return pmErrno = PM_OK; +} + +int pmQuatMatConvert(PmQuaternion const * const q, PmRotationMatrix * const m) +{ +#ifdef PM_DEBUG + if (!pmQuatIsNorm(q)) { +#ifdef PM_PRINT_ERROR + pmPrintError("Bad quaternion in pmQuatMatConvert\n"); +#endif + return pmErrno = PM_NORM_ERR; + } +#endif + + /* from space book where e1=q->x e2=q->y e3=q->z e4=q->s */ + m->x.x = 1.0 - 2.0 * (pmSq(q->y) + pmSq(q->z)); + m->y.x = 2.0 * (q->x * q->y - q->z * q->s); + m->z.x = 2.0 * (q->z * q->x + q->y * q->s); + + m->x.y = 2.0 * (q->x * q->y + q->z * q->s); + m->y.y = 1.0 - 2.0 * (pmSq(q->z) + pmSq(q->x)); + m->z.y = 2.0 * (q->y * q->z - q->x * q->s); + + m->x.z = 2.0 * (q->z * q->x - q->y * q->s); + m->y.z = 2.0 * (q->y * q->z + q->x * q->s); + m->z.z = 1.0 - 2.0 * (pmSq(q->x) + pmSq(q->y)); + + return pmErrno = PM_OK; +} + +int pmQuatZyzConvert(PmQuaternion const * const q, PmEulerZyz * const zyz) +{ + PmRotationMatrix m; + int r1, r2; + + /*! \todo FIXME-- need direct equations */ + r1 = pmQuatMatConvert(q, &m); + r2 = pmMatZyzConvert(&m, zyz); + + return pmErrno = (r1 || r2) ? PM_NORM_ERR : PM_OK; +} + +int pmQuatZyxConvert(PmQuaternion const * const q, PmEulerZyx * const zyx) +{ + PmRotationMatrix m; + int r1, r2; + + /*! \todo FIXME-- need direct equations */ + r1 = pmQuatMatConvert(q, &m); + r2 = pmMatZyxConvert(&m, zyx); + + return pmErrno = (r1 || r2) ? PM_NORM_ERR : PM_OK; +} + +int pmQuatRpyConvert(PmQuaternion const * const q, PmRpy * const rpy) +{ + PmRotationMatrix m; + int r1, r2; + + /*! \todo FIXME-- need direct equations */ + r1 = pmQuatMatConvert(q, &m); + r2 = pmMatRpyConvert(&m, rpy); + + return pmErrno = (r1 || r2) ? PM_NORM_ERR : PM_OK; +} + +int pmMatRotConvert(PmRotationMatrix const * const m, PmRotationVector * const r) +{ + PmQuaternion q; + int r1, r2; + + /*! \todo FIXME-- need direct equations */ + r1 = pmMatQuatConvert(m, &q); + r2 = pmQuatRotConvert(&q, r); + + return pmErrno = (r1 || r2) ? PM_NORM_ERR : PM_OK; +} + +int pmMatQuatConvert(PmRotationMatrix const * const m, PmQuaternion * const q) +{ + /* + from Stephe's "space" book e1 = (c32 - c23) / 4*e4 e2 = (c13 - c31) / + 4*e4 e3 = (c21 - c12) / 4*e4 e4 = sqrt(1 + c11 + c22 + c33) / 2 + + if e4 == 0 e1 = sqrt(1 + c11 - c33 - c22) / 2 e2 = sqrt(1 + c22 - c33 + - c11) / 2 e3 = sqrt(1 + c33 - c11 - c22) / 2 to determine whether to + take the positive or negative sqrt value since e4 == 0 indicates a + 180* rotation then (0 x y z) = (0 -x -y -z). Thus some generallities + can be used: 1) find which of e1, e2, or e3 has the largest magnitude + and leave it pos. 2) if e1 is largest then if c21 < 0 then take the + negative for e2 if c31 < 0 then take the negative for e3 3) else if e2 + is largest then if c21 < 0 then take the negative for e1 if c32 < 0 + then take the negative for e3 4) else if e3 is larger then if c31 < 0 + then take the negative for e1 if c32 < 0 then take the negative for e2 + + Note: c21 in the space book is m->x.y in this C code */ + + double a; + +#ifdef PM_DEBUG + if (!pmMatIsNorm(m)) { +#ifdef PM_PRINT_ERROR + pmPrintError("Bad matrix in pmMatQuatConvert\n"); +#endif + return pmErrno = PM_NORM_ERR; + } +#endif + + q->s = 0.5 * pmSqrt(1.0 + m->x.x + m->y.y + m->z.z); + + if (fabs(q->s) > QS_FUZZ) { + q->x = (m->y.z - m->z.y) / (a = 4 * q->s); + q->y = (m->z.x - m->x.z) / a; + q->z = (m->x.y - m->y.x) / a; + } else { + q->s = 0; + q->x = pmSqrt(1.0 + m->x.x - m->y.y - m->z.z) / 2.0; + q->y = pmSqrt(1.0 + m->y.y - m->x.x - m->z.z) / 2.0; + q->z = pmSqrt(1.0 + m->z.z - m->y.y - m->x.x) / 2.0; + + if (q->x > q->y && q->x > q->z) { + if (m->x.y < 0.0) { + q->y *= -1; + } + if (m->x.z < 0.0) { + q->z *= -1; + } + } else if (q->y > q->z) { + if (m->x.y < 0.0) { + q->x *= -1; + } + if (m->y.z < 0.0) { + q->z *= -1; + } + } else { + if (m->x.z < 0.0) { + q->x *= -1; + } + if (m->y.z < 0.0) { + q->y *= -1; + } + } + } + + pmErrno = PM_OK; + return pmQuatNorm(q, q); +} + +int pmMatZyzConvert(PmRotationMatrix const * const m, PmEulerZyz * const zyz) +{ + zyz->y = atan2(pmSqrt(pmSq(m->x.z) + pmSq(m->y.z)), m->z.z); + + if (fabs(zyz->y) < ZYZ_Y_FUZZ) { + zyz->z = 0.0; + zyz->y = 0.0; /* force Y to 0 */ + zyz->zp = atan2(-m->y.x, m->x.x); + } else if (fabs(zyz->y - PM_PI) < ZYZ_Y_FUZZ) { + zyz->z = 0.0; + zyz->y = PM_PI; /* force Y to 180 */ + zyz->zp = atan2(m->y.x, -m->x.x); + } else { + zyz->z = atan2(m->z.y, m->z.x); + zyz->zp = atan2(m->y.z, -m->x.z); + } + + return pmErrno = PM_OK; +} + +int pmMatZyxConvert(PmRotationMatrix const * const m, PmEulerZyx * const zyx) +{ + zyx->y = atan2(-m->x.z, pmSqrt(pmSq(m->x.x) + pmSq(m->x.y))); + + if (fabs(zyx->y - PM_PI_2) < ZYX_Y_FUZZ) { + zyx->z = 0.0; + zyx->y = PM_PI_2; /* force it */ + zyx->x = atan2(m->y.x, m->y.y); + } else if (fabs(zyx->y + PM_PI_2) < ZYX_Y_FUZZ) { + zyx->z = 0.0; + zyx->y = -PM_PI_2; /* force it */ + zyx->x = -atan2(m->y.z, m->y.y); + } else { + zyx->z = atan2(m->x.y, m->x.x); + zyx->x = atan2(m->y.z, m->z.z); + } + + return pmErrno = PM_OK; +} + +int pmMatRpyConvert(PmRotationMatrix const * const m, PmRpy * const rpy) +{ + rpy->p = atan2(-m->x.z, pmSqrt(pmSq(m->x.x) + pmSq(m->x.y))); + + if (fabs(rpy->p - PM_PI_2) < RPY_P_FUZZ) { + rpy->r = atan2(m->y.x, m->y.y); + rpy->p = PM_PI_2; /* force it */ + rpy->y = 0.0; + } else if (fabs(rpy->p + PM_PI_2) < RPY_P_FUZZ) { + rpy->r = -atan2(m->y.x, m->y.y); + rpy->p = -PM_PI_2; /* force it */ + rpy->y = 0.0; + } else { + rpy->r = atan2(m->y.z, m->z.z); + rpy->y = atan2(m->x.y, m->x.x); + } + + return pmErrno = PM_OK; +} + +int pmZyzRotConvert(PmEulerZyz const * const zyz, PmRotationVector * const r) +{ + (void)zyz; + (void)r; +#ifdef PM_PRINT_ERROR + pmPrintError("error: pmZyzRotConvert not implemented\n"); +#endif + return pmErrno = PM_IMPL_ERR; +} + +int pmZyzQuatConvert(PmEulerZyz const * const zyz, PmQuaternion * const q) +{ + PmRotationMatrix m; + int r1, r2; + + /*! \todo FIXME-- need direct equations */ + r1 = pmZyzMatConvert(zyz, &m); + r2 = pmMatQuatConvert(&m, q); + + return pmErrno = (r1 || r2) ? PM_NORM_ERR : PM_OK; +} + +int pmZyzMatConvert(PmEulerZyz const * const zyz, PmRotationMatrix * const m) +{ + double sa, sb, sg; + double ca, cb, cg; + + sa = sin(zyz->z); + sb = sin(zyz->y); + sg = sin(zyz->zp); + + ca = cos(zyz->z); + cb = cos(zyz->y); + cg = cos(zyz->zp); + + m->x.x = ca * cb * cg - sa * sg; + m->y.x = -ca * cb * sg - sa * cg; + m->z.x = ca * sb; + + m->x.y = sa * cb * cg + ca * sg; + m->y.y = -sa * cb * sg + ca * cg; + m->z.y = sa * sb; + + m->x.z = -sb * cg; + m->y.z = sb * sg; + m->z.z = cb; + + return pmErrno = PM_OK; +} + +int pmZyzRpyConvert(PmEulerZyz const * const zyz, PmRpy * const rpy) +{ + (void)zyz; + (void)rpy; +#ifdef PM_PRINT_ERROR + pmPrintError("error: pmZyzRpyConvert not implemented\n"); +#endif + return pmErrno = PM_IMPL_ERR; +} + +int pmZyxRotConvert(PmEulerZyx const * const zyx, PmRotationVector * const r) +{ + PmRotationMatrix m; + int r1, r2; + + /*! \todo FIXME-- need direct equations */ + r1 = pmZyxMatConvert(zyx, &m); + r2 = pmMatRotConvert(&m, r); + + return pmErrno = (r1 || r2) ? PM_NORM_ERR : PM_OK; +} + +int pmZyxQuatConvert(PmEulerZyx const * const zyx, PmQuaternion * const q) +{ + PmRotationMatrix m; + int r1, r2; + + /*! \todo FIXME-- need direct equations */ + r1 = pmZyxMatConvert(zyx, &m); + r2 = pmMatQuatConvert(&m, q); + + return pmErrno = (r1 || r2) ? PM_NORM_ERR : PM_OK; +} + +int pmZyxMatConvert(PmEulerZyx const * const zyx, PmRotationMatrix * const m) +{ + double sa, sb, sg; + double ca, cb, cg; + + sa = sin(zyx->z); + sb = sin(zyx->y); + sg = sin(zyx->x); + + ca = cos(zyx->z); + cb = cos(zyx->y); + cg = cos(zyx->x); + + m->x.x = ca * cb; + m->y.x = ca * sb * sg - sa * cg; + m->z.x = ca * sb * cg + sa * sg; + + m->x.y = sa * cb; + m->y.y = sa * sb * sg + ca * cg; + m->z.y = sa * sb * cg - ca * sg; + + m->x.z = -sb; + m->y.z = cb * sg; + m->z.z = cb * cg; + + return pmErrno = PM_OK; +} + +int pmZyxZyzConvert(PmEulerZyx const * const zyx, PmEulerZyz * const zyz) +{ + (void)zyx; + (void)zyz; +#ifdef PM_PRINT_ERROR + pmPrintError("error: pmZyxZyzConvert not implemented\n"); +#endif + return pmErrno = PM_IMPL_ERR; +} + +int pmZyxRpyConvert(PmEulerZyx const * const zyx, PmRpy * const rpy) +{ + (void)zyx; + (void)rpy; +#ifdef PM_PRINT_ERROR + pmPrintError("error: pmZyxRpyConvert not implemented\n"); +#endif + return pmErrno = PM_IMPL_ERR; +} + +int pmRpyRotConvert(PmRpy const * const rpy, PmRotationVector * const r) +{ + PmQuaternion q; + int r1, r2; + + q.s = q.x = q.y = q.z = 0.0; + r->s = r->x = r->y = r->z = 0.0; + + r1 = pmRpyQuatConvert(rpy, &q); + r2 = pmQuatRotConvert(&q, r); + + return (r1 || r2) ? pmErrno : PM_OK; +} + +int pmRpyQuatConvert(PmRpy const * const rpy, PmQuaternion * const q) +{ + PmRotationMatrix m; + int r1, r2; + + /*! \todo FIXME-- need direct equations */ + r1 = pmRpyMatConvert(rpy, &m); + r2 = pmMatQuatConvert(&m, q); + + return pmErrno = (r1 || r2) ? PM_NORM_ERR : PM_OK; +} + +int pmRpyMatConvert(PmRpy const * const rpy, PmRotationMatrix * const m) +{ + double sa, sb, sg; + double ca, cb, cg; + + sa = sin(rpy->y); + sb = sin(rpy->p); + sg = sin(rpy->r); + + ca = cos(rpy->y); + cb = cos(rpy->p); + cg = cos(rpy->r); + + m->x.x = ca * cb; + m->y.x = ca * sb * sg - sa * cg; + m->z.x = ca * sb * cg + sa * sg; + + m->x.y = sa * cb; + m->y.y = sa * sb * sg + ca * cg; + m->z.y = sa * sb * cg - ca * sg; + + m->x.z = -sb; + m->y.z = cb * sg; + m->z.z = cb * cg; + + return pmErrno = PM_OK; +} + +int pmRpyZyzConvert(PmRpy const * const rpy, PmEulerZyz * const zyz) +{ + (void)rpy; + (void)zyz; +#ifdef PM_PRINT_ERROR + pmPrintError("error: pmRpyZyzConvert not implemented\n"); +#endif + return pmErrno = PM_IMPL_ERR; +} + +int pmRpyZyxConvert(PmRpy const * const rpy, PmEulerZyx * const zyx) +{ + (void)rpy; + (void)zyx; +#ifdef PM_PRINT_ERROR + pmPrintError("error: pmRpyZyxConvert not implemented\n"); +#endif + return pmErrno = PM_IMPL_ERR; +} + +int pmPoseHomConvert(PmPose const * const p, PmHomogeneous * const h) +{ + int r1; + + h->tran = p->tran; + r1 = pmQuatMatConvert(&p->rot, &h->rot); + + return pmErrno = r1; +} + +int pmHomPoseConvert(PmHomogeneous const * const h, PmPose * const p) +{ + int r1; + + p->tran = h->tran; + r1 = pmMatQuatConvert(&h->rot, &p->rot); + + return pmErrno = r1; +} + +/* PmCartesian functions */ + +int pmCartCartCompare(PmCartesian const * const v1, PmCartesian const * const v2) +{ + if (fabs(v1->x - v2->x) >= V_FUZZ || + fabs(v1->y - v2->y) >= V_FUZZ || fabs(v1->z - v2->z) >= V_FUZZ) { + return 0; + } + + return 1; +} + +int pmCartCartDot(PmCartesian const * const v1, PmCartesian const * const v2, double *d) +{ + *d = v1->x * v2->x + v1->y * v2->y + v1->z * v2->z; + + return pmErrno = PM_OK; +} + +int pmCartCartMult(PmCartesian const * const v1, PmCartesian const * const v2, + PmCartesian * const out) +{ + out->x = v1->x * v2->x; + out->y = v1->y * v2->y; + out->z = v1->z * v2->z; + + return pmErrno = PM_OK; +} + +int pmCartCartDiv(PmCartesian const * const v1, PmCartesian const * const v2, + PmCartesian * const out) +{ + if (0.0==v2->x || 0.0==v2->y || 0.0==v2->z) { +#ifdef PM_PRINT_ERROR + pmPrintError(&"Divide by 0 in pmCartCartDiv\n"); +#endif + out->x = out->y = out->z = 0.0; + return pmErrno = PM_DIV_ERR; + } + out->x = v1->x / v2->x; + out->y = v1->y / v2->y; + out->z = v1->z / v2->z; + + return pmErrno = PM_OK; +} + +int pmCartCartCross(PmCartesian const * const v1, PmCartesian const * const v2, + PmCartesian * const vout) +{ + if (vout == v1 || vout == v2) { + return pmErrno = PM_IMPL_ERR; + } + vout->x = v1->y * v2->z - v1->z * v2->y; + vout->y = v1->z * v2->x - v1->x * v2->z; + vout->z = v1->x * v2->y - v1->y * v2->x; + + return pmErrno = PM_OK; +} + +int pmCartInfNorm(PmCartesian const * v, double * out) +{ + *out = fmax(fabs(v->x),fmax(fabs(v->y),fabs(v->z))); + + return pmErrno = PM_OK; +} + +int pmCartMag(PmCartesian const * const v, double *d) +{ + *d = pmSqrt(pmSq(v->x) + pmSq(v->y) + pmSq(v->z)); + + return pmErrno = PM_OK; +} + +/** Find square of magnitude of a vector (useful for some calculations to save a sqrt).*/ +int pmCartMagSq(PmCartesian const * const v, double *d) +{ + *d = pmSq(v->x) + pmSq(v->y) + pmSq(v->z); + + return pmErrno = PM_OK; +} + +int pmCartCartDisp(PmCartesian const * const v1, PmCartesian const * const v2, + double *d) +{ + *d = pmSqrt(pmSq(v2->x - v1->x) + pmSq(v2->y - v1->y) + pmSq(v2->z - v1->z)); + + return pmErrno = PM_OK; +} + +int pmCartCartAdd(PmCartesian const * const v1, PmCartesian const * const v2, + PmCartesian * const vout) +{ + vout->x = v1->x + v2->x; + vout->y = v1->y + v2->y; + vout->z = v1->z + v2->z; + + return pmErrno = PM_OK; +} + +int pmCartCartSub(PmCartesian const * const v1, PmCartesian const * const v2, + PmCartesian * const vout) +{ + vout->x = v1->x - v2->x; + vout->y = v1->y - v2->y; + vout->z = v1->z - v2->z; + + return pmErrno = PM_OK; +} + +int pmCartScalMult(PmCartesian const * const v1, double d, PmCartesian * const vout) +{ + if (v1 != vout) { + *vout = *v1; + } + + return pmCartScalMultEq(vout, d); +} + +int pmCartScalDiv(PmCartesian const * const v1, double d, PmCartesian * const vout) +{ + if (v1 != vout) { + *vout = *v1; + } + + return pmCartScalDivEq(vout, d); +} + +int pmCartNeg(PmCartesian const * const v1, PmCartesian * const vout) +{ + if (v1 != vout) { + *vout = *v1; + } + + return pmCartNegEq(vout); +} + +int pmCartNegEq(PmCartesian * const v1) +{ + v1->x = -v1->x; + v1->y = -v1->y; + v1->z = -v1->z; + + return pmErrno = PM_OK; +} + +int pmCartInv(PmCartesian const * const v1, PmCartesian * const vout) +{ + if (v1 != vout) { + *vout = *v1; + } + + return pmCartInvEq(vout); +} + +int pmCartInvEq(PmCartesian * const v) +{ + double size_sq; + pmCartMagSq(v,&size_sq); + + if (size_sq == 0.0) { +#ifdef PM_PRINT_ERROR + pmPrintError(&"Zero vector in pmCartInv\n"); +#endif + return pmErrno = PM_NORM_ERR; + } + + v->x /= size_sq; + v->y /= size_sq; + v->z /= size_sq; + + return pmErrno = PM_OK; +} + +// This used to be called pmCartNorm. + +int pmCartUnit(PmCartesian const * const v, PmCartesian * const vout) +{ + if (vout != v) { + *vout = *v; + } + return pmCartUnitEq(vout); +} + +int pmCartAbs(PmCartesian const * const v, PmCartesian * const vout) +{ + + vout->x = fabs(v->x); + vout->y = fabs(v->y); + vout->z = fabs(v->z); + + return pmErrno = PM_OK; +} + +/* Compound assign operator equivalent functions. These are to prevent issues with passing the same variable as both input (const) and output */ + +int pmCartCartAddEq(PmCartesian * const v, PmCartesian const * const v_add) +{ + v->x += v_add->x; + v->y += v_add->y; + v->z += v_add->z; + + return pmErrno = PM_OK; +} + +int pmCartCartSubEq(PmCartesian * const v, PmCartesian const * const v_sub) +{ + v->x -= v_sub->x; + v->y -= v_sub->y; + v->z -= v_sub->z; + + return pmErrno = PM_OK; +} + +int pmCartScalMultEq(PmCartesian * const v, double d) +{ + + v->x *= d; + v->y *= d; + v->z *= d; + + return pmErrno = PM_OK; +} + +int pmCartScalDivEq(PmCartesian * const v, double d) +{ + + if (d == 0.0) { +#ifdef PM_PRINT_ERROR + pmPrintError(&"Divide by 0 in pmCartScalDiv\n"); +#endif + return pmErrno = PM_DIV_ERR; + } + + v->x /= d; + v->y /= d; + v->z /= d; + + return pmErrno = PM_OK; +} + +int pmCartUnitEq(PmCartesian * const v) +{ + double size = pmSqrt(pmSq(v->x) + pmSq(v->y) + pmSq(v->z)); + + if (size == 0.0) { +#ifdef PM_PRINT_ERROR + pmPrintError("Zero vector in pmCartUnit\n"); +#endif + return pmErrno = PM_NORM_ERR; + } + + v->x /= size; + v->y /= size; + v->z /= size; + + return pmErrno = PM_OK; +} + +/*! \todo This is if 0'd out so we can find all the pmCartNorm calls that should + be renamed pmCartUnit. + Later we'll put this back. */ +#if 0 + +int pmCartNorm(PmCartesian const * const v, PmCartesian * const vout) +{ + + vout->x = v->x; + vout->y = v->y; + vout->z = v->z; + + return pmErrno = PM_OK; +} +#endif + +int pmCartIsNorm(PmCartesian const * const v) +{ + return pmSqrt(pmSq(v->x) + pmSq(v->y) + pmSq(v->z)) - 1.0 < UNIT_VEC_FUZZ; +} + +int pmCartCartProj(PmCartesian const * const v1, PmCartesian const * const v2, PmCartesian * const vout) +{ + int r1, r2; + int r3=1; + double d12; + double d22; + + r1 = pmCartCartDot(v1, v2, &d12); + r2 = pmCartCartDot(v2, v2, &d22); + if (!(r1 || r2)){ + r3 = pmCartScalMult(v2, d12/d22, vout); + } + + return pmErrno = (r1 || r2 || r3) ? PM_NORM_ERR : PM_OK; +} + +int pmCartPlaneProj(PmCartesian const * const v, PmCartesian const * const normal, PmCartesian * const vout) +{ + int r1, r2; + PmCartesian par; + + r1 = pmCartCartProj(v, normal, &par); + r2 = pmCartCartSub(v, &par, vout); + + return pmErrno = (r1 || r2) ? PM_NORM_ERR : PM_OK; +} + +/* angle-axis functions */ + +int pmQuatAxisAngleMult(PmQuaternion const * const q, PmAxis axis, double angle, + PmQuaternion * const pq) +{ + double sh, ch; + +#ifdef PM_DEBUG + if (!pmQuatIsNorm(q)) { +#ifdef PM_PRINT_ERROR + pmPrintError("error: non-unit quaternion in pmQuatAxisAngleMult\n"); +#endif + return pmErrno = PM_ERR; + } +#endif + + angle *= 0.5; + pm_sincos(angle, &sh, &ch); + + switch (axis) { + case PM_X: + pq->s = ch * q->s - sh * q->x; + pq->x = ch * q->x + sh * q->s; + pq->y = ch * q->y + sh * q->z; + pq->z = ch * q->z - sh * q->y; + break; + + case PM_Y: + pq->s = ch * q->s - sh * q->y; + pq->x = ch * q->x - sh * q->z; + pq->y = ch * q->y + sh * q->s; + pq->z = ch * q->z + sh * q->x; + break; + + case PM_Z: + pq->s = ch * q->s - sh * q->z; + pq->x = ch * q->x + sh * q->y; + pq->y = ch * q->y - sh * q->x; + pq->z = ch * q->z + sh * q->s; + break; + + default: +#ifdef PM_PRINT_ERROR + pmPrintError("error: bad axis in pmQuatAxisAngleMult\n"); +#endif + return pmErrno = PM_ERR; + } + + if (pq->s < 0.0) { + pq->s *= -1.0; + pq->x *= -1.0; + pq->y *= -1.0; + pq->z *= -1.0; + } + + return 0; +} + +/* PmRotationVector functions */ + +int pmRotScalMult(PmRotationVector const * const r, double s, PmRotationVector * const rout) +{ + rout->s = r->s * s; + rout->x = r->x; + rout->y = r->y; + rout->z = r->z; + + return pmErrno = PM_OK; +} + +int pmRotScalDiv(PmRotationVector const * const r, double s, PmRotationVector * const rout) +{ + if (s == 0.0) { +#ifdef PM_PRINT_ERROR + pmPrintError("Divide by zero in pmRotScalDiv\n"); +#endif + + rout->s = DBL_MAX; + rout->x = r->x; + rout->y = r->y; + rout->z = r->z; + + return pmErrno = PM_NORM_ERR; + } + + rout->s = r->s / s; + rout->x = r->x; + rout->y = r->y; + rout->z = r->z; + + return pmErrno = PM_OK; +} + +int pmRotIsNorm(PmRotationVector const * const r) +{ + if (fabs(r->s) < RS_FUZZ || + fabs(pmSqrt(pmSq(r->x) + pmSq(r->y) + pmSq(r->z))) - 1.0 < UNIT_VEC_FUZZ) + { + return 1; + } + + return 0; +} + +int pmRotNorm(PmRotationVector const * const r, PmRotationVector * const rout) +{ + double size; + + size = pmSqrt(pmSq(r->x) + pmSq(r->y) + pmSq(r->z)); + + if (fabs(r->s) < RS_FUZZ) { + rout->s = 0.0; + rout->x = 0.0; + rout->y = 0.0; + rout->z = 0.0; + + return pmErrno = PM_OK; + } + + if (size == 0.0) { +#ifdef PM_PRINT_ERROR + pmPrintError("error: pmRotNorm size is zero\n"); +#endif + + rout->s = 0.0; + rout->x = 0.0; + rout->y = 0.0; + rout->z = 0.0; + + return pmErrno = PM_NORM_ERR; + } + + rout->s = r->s; + rout->x = r->x / size; + rout->y = r->y / size; + rout->z = r->z / size; + + return pmErrno = PM_OK; +} + +/* PmRotationMatrix functions */ + +int pmMatNorm(PmRotationMatrix const * const m, PmRotationMatrix * const mout) +{ + /*! \todo FIXME */ + *mout = *m; + +#ifdef PM_PRINT_ERROR + pmPrintError("error: pmMatNorm not implemented\n"); +#endif + return pmErrno = PM_IMPL_ERR; +} + +int pmMatIsNorm(PmRotationMatrix const * const m) +{ + PmCartesian u; + + pmCartCartCross(&m->x, &m->y, &u); + + return (pmCartIsNorm(&m->x) && pmCartIsNorm(&m->y) && pmCartIsNorm(&m->z) && pmCartCartCompare(&u, &m->z)); +} + +int pmMatInv(PmRotationMatrix const * const m, PmRotationMatrix * const mout) +{ + /* inverse of a rotation matrix is the transpose */ + + mout->x.x = m->x.x; + mout->x.y = m->y.x; + mout->x.z = m->z.x; + + mout->y.x = m->x.y; + mout->y.y = m->y.y; + mout->y.z = m->z.y; + + mout->z.x = m->x.z; + mout->z.y = m->y.z; + mout->z.z = m->z.z; + + return pmErrno = PM_OK; +} + +int pmMatCartMult(PmRotationMatrix const * const m, PmCartesian const * const v, PmCartesian * const vout) +{ + vout->x = m->x.x * v->x + m->y.x * v->y + m->z.x * v->z; + vout->y = m->x.y * v->x + m->y.y * v->y + m->z.y * v->z; + vout->z = m->x.z * v->x + m->y.z * v->y + m->z.z * v->z; + + return pmErrno = PM_OK; +} + +int pmMatMatMult(PmRotationMatrix const * const m1, PmRotationMatrix const * const m2, + PmRotationMatrix * const mout) +{ + mout->x.x = m1->x.x * m2->x.x + m1->y.x * m2->x.y + m1->z.x * m2->x.z; + mout->x.y = m1->x.y * m2->x.x + m1->y.y * m2->x.y + m1->z.y * m2->x.z; + mout->x.z = m1->x.z * m2->x.x + m1->y.z * m2->x.y + m1->z.z * m2->x.z; + + mout->y.x = m1->x.x * m2->y.x + m1->y.x * m2->y.y + m1->z.x * m2->y.z; + mout->y.y = m1->x.y * m2->y.x + m1->y.y * m2->y.y + m1->z.y * m2->y.z; + mout->y.z = m1->x.z * m2->y.x + m1->y.z * m2->y.y + m1->z.z * m2->y.z; + + mout->z.x = m1->x.x * m2->z.x + m1->y.x * m2->z.y + m1->z.x * m2->z.z; + mout->z.y = m1->x.y * m2->z.x + m1->y.y * m2->z.y + m1->z.y * m2->z.z; + mout->z.z = m1->x.z * m2->z.x + m1->y.z * m2->z.y + m1->z.z * m2->z.z; + + return pmErrno = PM_OK; +} + +/* PmQuaternion functions */ + +int pmQuatQuatCompare(PmQuaternion const * const q1, PmQuaternion const * const q2) +{ +#ifdef PM_DEBUG + if (!pmQuatIsNorm(q1) || !pmQuatIsNorm(q2)) { +#ifdef PM_PRINT_ERROR + pmPrintError("Bad quaternion in pmQuatQuatCompare\n"); +#endif + return pmErrno = PM_NORM_ERR; + } +#endif + + if (fabs(q1->s - q2->s) < Q_FUZZ && + fabs(q1->x - q2->x) < Q_FUZZ && + fabs(q1->y - q2->y) < Q_FUZZ && fabs(q1->z - q2->z) < Q_FUZZ) { + return 1; + } + + /* note (0, x, y, z) = (0, -x, -y, -z) */ + if (fabs(q1->s) >= QS_FUZZ || + fabs(q1->x + q2->x) >= Q_FUZZ || + fabs(q1->y + q2->y) >= Q_FUZZ || fabs(q1->z + q2->z) >= Q_FUZZ) { + return 0; + } + + return 1; +} + +int pmQuatMag(PmQuaternion const * const q, double *d) +{ + PmRotationVector r; + int r1; + + if (0 == d) { + return pmErrno = PM_ERR; + } + + r1 = pmQuatRotConvert(q, &r); + *d = r.s; + + return pmErrno = r1; +} + +int pmQuatNorm(PmQuaternion const * const q1, PmQuaternion * const qout) +{ + double size = pmSqrt(pmSq(q1->s) + pmSq(q1->x) + pmSq(q1->y) + pmSq(q1->z)); + + if (size == 0.0) { +#ifdef PM_PRINT_ERROR + pmPrintError("Bad quaternion in pmQuatNorm\n"); +#endif + qout->s = 1; + qout->x = 0; + qout->y = 0; + qout->z = 0; + + return pmErrno = PM_NORM_ERR; + } + + if (q1->s >= 0.0) { + qout->s = q1->s / size; + qout->x = q1->x / size; + qout->y = q1->y / size; + qout->z = q1->z / size; + + return pmErrno = PM_OK; + } else { + qout->s = -q1->s / size; + qout->x = -q1->x / size; + qout->y = -q1->y / size; + qout->z = -q1->z / size; + + return pmErrno = PM_OK; + } +} + +int pmQuatInv(PmQuaternion const * const q1, PmQuaternion * const qout) +{ + if (qout == 0) { + return pmErrno = PM_ERR; + } + + qout->s = q1->s; + qout->x = -q1->x; + qout->y = -q1->y; + qout->z = -q1->z; + +#ifdef PM_DEBUG + if (!pmQuatIsNorm(q1)) { +#ifdef PM_PRINT_ERROR + pmPrintError("Bad quaternion in pmQuatInv\n"); +#endif + return pmErrno = PM_NORM_ERR; + } +#endif + + return pmErrno = PM_OK; +} + +int pmQuatIsNorm(PmQuaternion const * const q1) +{ + return (fabs(pmSq(q1->s) + pmSq(q1->x) + pmSq(q1->y) + pmSq(q1->z) - 1.0) < + UNIT_QUAT_FUZZ); +} + +int pmQuatScalMult(PmQuaternion const * const q, double s, PmQuaternion * const qout) +{ + /*! \todo FIXME-- need a native version; this goes through a rotation vector */ + PmRotationVector r; + int r1, r2, r3; + + r1 = pmQuatRotConvert(q, &r); + r2 = pmRotScalMult(&r, s, &r); + r3 = pmRotQuatConvert(&r, qout); + + return pmErrno = (r1 || r2 || r3) ? PM_NORM_ERR : PM_OK; +} + +int pmQuatScalDiv(PmQuaternion const * const q, double s, PmQuaternion * const qout) +{ + /*! \todo FIXME-- need a native version; this goes through a rotation vector */ + PmRotationVector r; + int r1, r2, r3; + + r1 = pmQuatRotConvert(q, &r); + r2 = pmRotScalDiv(&r, s, &r); + r3 = pmRotQuatConvert(&r, qout); + + return pmErrno = (r1 || r2 || r3) ? PM_NORM_ERR : PM_OK; +} + +int pmQuatQuatMult(PmQuaternion const * const q1, PmQuaternion const * const q2, PmQuaternion * const qout) +{ + if (qout == 0) { + return pmErrno = PM_ERR; + } + + qout->s = q1->s * q2->s - q1->x * q2->x - q1->y * q2->y - q1->z * q2->z; + + if (qout->s >= 0.0) { + qout->x = q1->s * q2->x + q1->x * q2->s + q1->y * q2->z - q1->z * q2->y; + qout->y = q1->s * q2->y - q1->x * q2->z + q1->y * q2->s + q1->z * q2->x; + qout->z = q1->s * q2->z + q1->x * q2->y - q1->y * q2->x + q1->z * q2->s; + } else { + qout->s *= -1; + qout->x = -q1->s * q2->x - q1->x * q2->s - q1->y * q2->z + q1->z * q2->y; + qout->y = -q1->s * q2->y + q1->x * q2->z - q1->y * q2->s - q1->z * q2->x; + qout->z = -q1->s * q2->z - q1->x * q2->y + q1->y * q2->x - q1->z * q2->s; + } + +#ifdef PM_DEBUG + if (!pmQuatIsNorm(q1) || !pmQuatIsNorm(q2)) { +#ifdef PM_PRINT_ERROR + pmPrintError("Bad quaternion in pmQuatQuatMult\n"); +#endif + return pmErrno = PM_NORM_ERR; + } +#endif + + return pmErrno = PM_OK; +} + +int pmQuatCartMult(PmQuaternion const * const q1, PmCartesian const * const v2, PmCartesian * const vout) +{ + PmCartesian c; + + c.x = q1->y * v2->z - q1->z * v2->y; + c.y = q1->z * v2->x - q1->x * v2->z; + c.z = q1->x * v2->y - q1->y * v2->x; + + vout->x = v2->x + 2.0 * (q1->s * c.x + q1->y * c.z - q1->z * c.y); + vout->y = v2->y + 2.0 * (q1->s * c.y + q1->z * c.x - q1->x * c.z); + vout->z = v2->z + 2.0 * (q1->s * c.z + q1->x * c.y - q1->y * c.x); + +#ifdef PM_DEBUG + if (!pmQuatIsNorm(q1)) { +#ifdef PM_PRINT_ERROR + pmPrintError(&"Bad quaternion in pmQuatCartMult\n"); +#endif + return pmErrno = PM_NORM_ERR; + } +#endif + + return pmErrno = PM_OK; +} + +/* PmPose functions*/ + +int pmPosePoseCompare(PmPose const * const p1, PmPose const * const p2) +{ +#ifdef PM_DEBUG + if (!pmQuatIsNorm(&p1->rot) || !pmQuatIsNorm(&p2->rot)) { +#ifdef PM_PRINT_ERROR + pmPrintError("Bad quaternion in pmPosePoseCompare\n"); +#endif + } +#endif + + return pmQuatQuatCompare(&p1->rot, &p2->rot) && pmCartCartCompare(&p1->tran, &p2->tran); +} + +int pmPoseInv(PmPose const * const p1, PmPose * const p2) +{ + int r1, r2; + +#ifdef PM_DEBUG + if (!pmQuatIsNorm(&p1->rot)) { +#ifdef PM_PRINT_ERROR + pmPrintError("Bad quaternion in pmPoseInv\n"); +#endif + } +#endif + + r1 = pmQuatInv(&p1->rot, &p2->rot); + r2 = pmQuatCartMult(&p2->rot, &p1->tran, &p2->tran); + + p2->tran.x *= -1.0; + p2->tran.y *= -1.0; + p2->tran.z *= -1.0; + + return pmErrno = (r1 || r2) ? PM_NORM_ERR : PM_OK; +} + +int pmPoseCartMult(PmPose const * const p1, PmCartesian const * const v2, PmCartesian * const vout) +{ + int r1, r2; + +#ifdef PM_DEBUG + if (!pmQuatIsNorm(&p1->rot)) { +#ifdef PM_PRINT_ERROR + pmPrintError(&"Bad quaternion in pmPoseCartMult\n"); +#endif + return pmErrno = PM_NORM_ERR; + } +#endif + + r1 = pmQuatCartMult(&p1->rot, v2, vout); + r2 = pmCartCartAdd(&p1->tran, vout, vout); + + return pmErrno = (r1 || r2) ? PM_NORM_ERR : PM_OK; +} + +int pmPosePoseMult(PmPose const * const p1, PmPose const * const p2, PmPose * const pout) +{ + int r1, r2, r3; + +#ifdef PM_DEBUG + if (!pmQuatIsNorm(&p1->rot) || !pmQuatIsNorm(&p2->rot)) { +#ifdef PM_PRINT_ERROR + pmPrintError("Bad quaternion in pmPosePoseMult\n"); +#endif + return pmErrno = PM_NORM_ERR; + } +#endif + + r1 = pmQuatCartMult(&p1->rot, &p2->tran, &pout->tran); + r2 = pmCartCartAdd(&p1->tran, &pout->tran, &pout->tran); + r3 = pmQuatQuatMult(&p1->rot, &p2->rot, &pout->rot); + + return pmErrno = (r1 || r2 || r3) ? PM_NORM_ERR : PM_OK; +} + +/* homogeneous transform functions */ + +int pmHomInv(PmHomogeneous const * const h1, PmHomogeneous * const h2) +{ + int r1, r2; + +#ifdef PM_DEBUG + if (!pmMatIsNorm(&h1->rot)) { +#ifdef PM_PRINT_ERROR + pmPrintError("Bad rotation matrix in pmHomInv\n"); +#endif + } +#endif + + r1 = pmMatInv(&h1->rot, &h2->rot); + r2 = pmMatCartMult(&h2->rot, &h1->tran, &h2->tran); + + h2->tran.x *= -1.0; + h2->tran.y *= -1.0; + h2->tran.z *= -1.0; + + return pmErrno = (r1 || r2) ? PM_NORM_ERR : PM_OK; +} + +/* line functions */ + +int pmLineInit(PmLine * const line, PmPose const * const start, PmPose const * const end) +{ + int r1, r2, r3, r4, r5; + r1 = r2 = r3 = r4 = r5 = PM_OK; + + double tmag = 0.0; + double rmag = 0.0; + PmQuaternion startQuatInverse; + + if (0 == line) { + return pmErrno = PM_ERR; + } + + r3 = pmQuatInv(&start->rot, &startQuatInverse); + if (r3) { + pmErrno = PM_NORM_ERR; + return r3; + } + + r4 = pmQuatQuatMult(&startQuatInverse, &end->rot, &line->qVec); + if (r4) { + pmErrno = PM_NORM_ERR; + return r4; + } + + pmQuatMag(&line->qVec, &rmag); + if (rmag > Q_FUZZ) { + r5 = pmQuatScalMult(&line->qVec, 1 / rmag, &(line->qVec)); + if (r5) { + pmErrno = PM_NORM_ERR; + return r5; + } + } + + line->start = *start; + line->end = *end; + r1 = pmCartCartSub(&end->tran, &start->tran, &line->uVec); + if (r1) { + pmErrno = PM_NORM_ERR; + return r1; + } + + pmCartMag(&line->uVec, &tmag); + if (IS_FUZZ(tmag, CART_FUZZ)) { + line->uVec.x = 1.0; + line->uVec.y = 0.0; + line->uVec.z = 0.0; + } else { + r2 = pmCartUnit(&line->uVec, &line->uVec); + } + line->tmag = tmag; + line->rmag = rmag; + line->tmag_zero = (line->tmag <= CART_FUZZ); + line->rmag_zero = (line->rmag <= Q_FUZZ); + + /* return PM_NORM_ERR if uVec has been set to 1, 0, 0 */ + return pmErrno = (r1 || r2 || r3 || r4 || r5) ? PM_NORM_ERR : PM_OK; +} + +int pmLinePoint(PmLine const * const line, double len, PmPose * const point) +{ + int r1, r2, r3, r4; + r1 = r2 = r3 = r4 = PM_OK; + + if (line->tmag_zero) { + point->tran = line->end.tran; + } else { + /* return start + len * uVec */ + r1 = pmCartScalMult(&line->uVec, len, &point->tran); + r2 = pmCartCartAdd(&line->start.tran, &point->tran, &point->tran); + } + + if (line->rmag_zero) { + point->rot = line->end.rot; + } else { + if (line->tmag_zero) { + r3 = pmQuatScalMult(&line->qVec, len, &point->rot); + } else { + r3 = pmQuatScalMult(&line->qVec, len * line->rmag / line->tmag, + &point->rot); + } + r4 = pmQuatQuatMult(&line->start.rot, &point->rot, &point->rot); + } + + return pmErrno = (r1 || r2 || r3 || r4) ? PM_NORM_ERR : PM_OK; +} + + +/* pure cartesian line functions */ + +int pmCartLineInit(PmCartLine * const line, PmCartesian const * const start, PmCartesian const * const end) +{ + int r1 = PM_OK, r2 = PM_OK; + + if (0 == line) { + return pmErrno = PM_ERR; + } + + line->start = *start; + line->end = *end; + r1 = pmCartCartSub(end, start, &line->uVec); + if (r1) { + pmErrno = PM_NORM_ERR; + return r1; + } + + pmCartMag(&line->uVec, &line->tmag); + // NOTE: use the same criteria for "zero" length vectors as used by canon + double max_xyz=0.0; + pmCartInfNorm(&line->uVec, &max_xyz); + + if (IS_FUZZ(max_xyz, CART_FUZZ)) { + line->uVec.x = 1.0; + line->uVec.y = 0.0; + line->uVec.z = 0.0; + line->tmag_zero = 1; + } else { + r2 = pmCartUnitEq(&line->uVec); + line->tmag_zero = 0; + } + + /* return PM_NORM_ERR if uVec has been set to 1, 0, 0 */ + return pmErrno = (r1 || r2) ? PM_NORM_ERR : PM_OK; +} + +int pmCartLinePoint(PmCartLine const * const line, double len, PmCartesian * const point) +{ + int r1 = PM_OK, r2 = PM_OK; + + if (line->tmag_zero) { + *point = line->end; + } else { + /* return start + len * uVec */ + r1 = pmCartScalMult(&line->uVec, len, point); + r2 = pmCartCartAdd(&line->start, point, point); + } + + return pmErrno = (r1 || r2) ? PM_NORM_ERR : PM_OK; +} + + +int pmCartLineStretch(PmCartLine * const line, double new_len, int from_end) +{ + int r1 = PM_OK, r2 = PM_OK; + + if (!line || line->tmag_zero || new_len <= DOUBLE_FUZZ) { + return pmErrno = PM_ERR; + } + + if (from_end) { + // Store the new relative position from end in the start point + r1 = pmCartScalMult(&line->uVec, -new_len, &line->start); + // Offset the new start point by the current end point + r2 = pmCartCartAddEq(&line->start, &line->end); + } else { + // Store the new relative position from start in the end point: + r1 = pmCartScalMult(&line->uVec, new_len, &line->end); + // Offset the new end point by the current start point + r2 = pmCartCartAdd(&line->start, &line->end, &line->end); + } + line->tmag = new_len; + + return pmErrno = (r1 || r2) ? PM_NORM_ERR : PM_OK; +} + +/* circle functions */ + +/* + pmCircleInit() takes the defining parameters of a generalized circle + and sticks them in the structure. It also computes the radius and vectors + in the plane that are useful for other functions and that don't need + to be recomputed every time. + + Note that the end can be placed arbitrarily, resulting in a combination of + spiral and helical motion. There is an overconstraint between the start, + center, and normal vector: the center vector and start vector are assumed + to be in the plane defined by the normal vector. If this is not true, then + it will be made true by moving the center vector onto the plane. + */ +int pmCircleInit(PmCircle * const circle, + PmCartesian const * const start, PmCartesian const * const end, + PmCartesian const * const center, PmCartesian const * const normal, int turn) +{ + double dot; + PmCartesian rEnd; + PmCartesian v; + double d; + int r1; + PmCartesian p1; + PmCartesian p2; + +#ifdef PM_DEBUG + if (0 == circle) { +#ifdef PM_PRINT_ERROR + pmPrintError("error: pmCircleInit circle pointer is null\n"); +#endif + return pmErrno = PM_ERR; + } +#endif + + /* adjust center */ + pmCartCartSub(start, center, &v); + r1 = pmCartCartProj(&v, normal, &v); + if (PM_NORM_ERR == r1) { + /* bad normal vector-- abort */ +#ifdef PM_PRINT_ERROR + pmPrintError("error: pmCircleInit normal vector is 0\n"); +#endif + return pmErrno = PM_ERR; + } + pmCartCartAdd(&v, center, &circle->center); + + /* normalize and redirect normal vector based on turns. If turn is less + than 0, point normal vector in other direction and make turn positive, + -1 -> 0, -2 -> 1, etc. */ + pmCartUnit(normal, &circle->normal); + if (turn < 0) { + turn = -1 - turn; + pmCartScalMult(&circle->normal, -1.0, &circle->normal); + } + + /* radius */ + pmCartCartDisp(start, &circle->center, &circle->radius); + + /* vector in plane of circle from center to start, magnitude radius */ + pmCartCartSub(start, &circle->center, &circle->rTan); + /* vector in plane of circle perpendicular to rTan, magnitude radius */ + pmCartCartCross(&circle->normal, &circle->rTan, &circle->rPerp); + + /* do rHelix, rEnd */ + pmCartCartSub(end, &circle->center, &circle->rHelix); + pmCartPlaneProj(&circle->rHelix, &circle->normal, &rEnd); + pmCartMag(&rEnd, &circle->spiral); + circle->spiral -= circle->radius; + pmCartCartSub(&circle->rHelix, &rEnd, &circle->rHelix); + pmCartUnit(&rEnd, &rEnd); + pmCartScalMult(&rEnd, circle->radius, &rEnd); + + /* Patch for error spiral end same as spiral center */ + pmCartMag(&rEnd, &d); + if (d == 0.0) { + pmCartScalMult(&circle->normal, DOUBLE_FUZZ, &v); + pmCartCartAdd(&rEnd, &v, &rEnd); + } + /* end patch 03-mar-1999 Dirk Maij */ + + /* angle */ + pmCartCartDot(&circle->rTan, &rEnd, &dot); + dot = dot / (circle->radius * circle->radius); + if (dot > 1.0) { + circle->angle = 0.0; + } else if (dot < -1.0) { + circle->angle = PM_PI; + } else { + circle->angle = acos(dot); + } + + /* now angle is in range 0..PI . Check if cross is antiparallel to + normal. If so, true angle is between PI..2PI. Need to subtract from + 2PI. */ + pmCartCartCross(&circle->rTan, &rEnd, &v); + pmCartCartDot(&v, &circle->normal, &d); + if (d < CART_FUZZ) { + circle->angle = PM_2_PI - circle->angle; + } + /* Issue #1528 24-Jan-2022. Additional test for nearly-straight * + * short arcs of very large radius becoming circles * + * Revisited 4-Dec-2022 Issue #2169 - endpoints of spirals are not* + * close to each other, project to working plane */ + pmCartPlaneProj(start, normal, &p1); + pmCartPlaneProj(end, normal, &p2); + pmCartCartDisp(&p1, &p2, &d); + if (d < CART_FUZZ){ + circle->angle = PM_2_PI; + } + + /* now add more angle for multi turns */ + if (turn > 0) { + circle->angle += turn * 2.0 * PM_PI; + } + + /* FIXME: some code has an unguarded division by circle->angle */ + if (circle->angle == 0) circle->angle += CIRCLE_FUZZ / 2; + + //Default to invalid +/* if 0'ed out while not debugging*/ +#if 0 + printf("\n\n"); + printf("pmCircleInit:\n"); + printf(" \t start : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n", + start->x, start->y, start->z); + printf(" \t end : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n", + end->x, end->y, end->z); + printf(" \t center : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n", + center->x, center->y, center->z); + printf(" \t normal : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n", + normal->x, normal->y, normal->z); + printf(" \t rEnd : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n", + rEnd.x, rEnd.y, rEnd.z); + printf(" \t turn=%d\n", turn); + printf(" \t dot=%9.9f\n", dot); + printf(" \t d=%9.9f\n", d); + printf(" \t circle \t{angle=%9.9f, radius=%9.9f, spiral=%9.9f}\n", + circle->angle, circle->radius, circle->spiral); + printf(" \t circle->normal : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n", + circle->normal.x, circle->normal.y, circle->normal.z); + printf(" \t circle->center : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n", + circle->center.x, circle->center.y, circle->center.z); + printf(" \t circle->rTan : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n", + circle->rTan.x, circle->rTan.y, circle->rTan.z); + printf(" \t circle->rPerp : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n", + circle->rPerp.x, circle->rPerp.y, circle->rPerp.z); + printf(" \t circle->rHelix : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n", + circle->rHelix.x, circle->rHelix.y, circle->rHelix.z); + printf("\n\n"); +#endif + + return pmErrno = PM_OK; +} + + +/* + pmCirclePoint() returns the point at the given angle along + the circle. If the circle is a helix or spiral or combination, the + point will include interpolation off the actual circle. + */ +int pmCirclePoint(PmCircle const * const circle, double angle, PmCartesian * const point) +{ + PmCartesian par, perp; + double scale; + +#ifdef PM_DEBUG + if (0 == circle || 0 == point) { +#ifdef PM_PRINT_ERROR + pmPrintError + ("error: pmCirclePoint circle or point pointer is null\n"); +#endif + return pmErrno = PM_ERR; + } +#endif + + /* compute components rel to center */ + pmCartScalMult(&circle->rTan, cos(angle), &par); + pmCartScalMult(&circle->rPerp, sin(angle), &perp); + + /* add to get radius vector rel to center */ + pmCartCartAdd(&par, &perp, point); + + /* get scale for spiral, helix interpolation */ + if (circle->angle == 0.0) { +#ifdef PM_PRINT_ERROR + pmPrintError("error: pmCirclePoint angle is zero\n"); +#endif + return pmErrno = PM_DIV_ERR; + } + scale = angle / circle->angle; + + /* add scaled vector in radial dir for spiral */ + pmCartUnit(point, &par); + pmCartScalMult(&par, scale * circle->spiral, &par); + pmCartCartAdd(point, &par, point); + + /* add scaled vector in helix dir */ + pmCartScalMult(&circle->rHelix, scale, &perp); + pmCartCartAdd(point, &perp, point); + + /* add to center vector for final result */ + pmCartCartAdd(&circle->center, point, point); + + return pmErrno = PM_OK; +} + +int pmCircleStretch(PmCircle * const circ, double new_angle, int from_end) +{ + if (!circ || new_angle <= DOUBLE_FUZZ) { + return pmErrno = PM_ERR; + } + + double mag = 0; + pmCartMagSq(&circ->rHelix, &mag); + if ( mag > 1e-6 ) { + //Can't handle helices + return pmErrno = PM_ERR; + } + //TODO handle spiral? + if (from_end) { + //Not implemented yet, way more reprocessing... + PmCartesian new_start; + double start_angle = circ->angle - new_angle; + pmCirclePoint(circ, start_angle, &new_start); + pmCartCartSub(&new_start, &circ->center, &circ->rTan); + pmCartCartCross(&circ->normal, &circ->rTan, &circ->rPerp); + pmCartMag(&circ->rTan, &circ->radius); + } + //Reduce the spiral proportionally + circ->spiral *= (new_angle / circ->angle); + // Easy to grow / shrink from start + circ->angle = new_angle; + + return pmErrno = PM_OK; +} diff --git a/wasm-port/vendor/linuxcnc/src/libnml/posemath/gomath.c b/wasm-port/vendor/linuxcnc/src/libnml/posemath/gomath.c new file mode 100644 index 0000000..cc31b8f --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/libnml/posemath/gomath.c @@ -0,0 +1,3743 @@ +/******************************************************************** +* Description: gomath.c +* Library file with various functions for working with matrices +* +* Derived from a work by Fred Proctor, +* changed to work with emc2 and HAL +* +* Adapting Author: Alex Joni +* License: LGPL Version 2 +* System: Linux +* +******************************************************************* + Similar to posemath, but using different functions. + + TODO: + * find the new functions, add them to posemath, convert the rest +*/ + +/* for debugging */ +extern int printf(const char * fmt, ...); +#include /* NULL */ +#include +#include /* memset */ +#include + +#include "sincos.h" + +#ifdef HAVE_CONFIG_H +#include "config.h" +#endif +#include "gotypes.h" /* GoResult ,Real, etc */ +#include "gomath.h" /* these decls, GoCartesian, etc. */ + +go_real go_cbrt(go_real x) +{ + if (x < 0.0) { + return (go_real) -pow((double) -x, 1.0/3.0); + } + return (go_real) pow((double) x, 1.0/3.0); +} + + +/* Translation rep conversion functions */ + +int go_cart_sph_convert(const go_cart * v, go_sph * s) +{ + go_real r; + + s->theta = atan2(v->y, v->x); + r = sqrt(go_sq(v->x) + go_sq(v->y) + go_sq(v->z)); + s->r = r; + if (GO_TRAN_SMALL(r)) { + s->phi = 0; + } else { + s->phi = acos(v->z / r); + } + + return GO_RESULT_OK; +} + +int go_cart_cyl_convert(const go_cart * v, go_cyl * c) +{ + c->theta = atan2(v->y, v->x); + c->r = sqrt(go_sq(v->x) + go_sq(v->y)); + c->z = v->z; + + return GO_RESULT_OK; +} + +int go_sph_cart_convert(const go_sph * s, go_cart * v) +{ + go_real sth, cth, sph, cph; + + pm_sincos(s->theta, &sth, &cth); + pm_sincos(s->phi, &sph, &cph); + + v->x = s->r * cth * sph; + v->y = s->r * sth * sph; + v->z = s->r * cph; + + return GO_RESULT_OK; +} + +int go_sph_cyl_convert(const go_sph * s, go_cyl * c) +{ + go_real sph, cph; + + pm_sincos(s->phi, &sph, &cph); + + c->theta = s->theta; + c->r = s->r * sph; + c->z = s->r * cph; + + return GO_RESULT_OK; +} + +int go_cyl_cart_convert(const go_cyl * c, go_cart * v) +{ + v->x = c->r * cos(c->theta); + v->y = c->r * sin(c->theta); + v->z = c->z; + + return GO_RESULT_OK; +} + +int go_cyl_sph_convert(const go_cyl * c, go_sph * s) +{ + s->theta = c->theta; + s->r = sqrt(go_sq(c->r) + go_sq(c->z)); + if (GO_TRAN_SMALL(s->r)) { + s->phi = 0.0; + } else { + s->phi = acos(c->z / s->r); + } + + return GO_RESULT_OK; +} + +/* rotation rep conversion functions */ + +int go_rvec_quat_convert(const go_rvec * r, go_quat * q) +{ + go_cart vec; + go_cart uvec; + go_real mag; + go_real sh; + + vec.x = r->x; + vec.y = r->y; + vec.z = r->z; + + if (GO_RESULT_OK != go_cart_unit(&vec, &uvec)) { + /* a zero vector */ + q->s = 1; + q->x = q->y = q->z = 0; + return GO_RESULT_OK; + } + + (void) go_cart_mag(&vec, &mag); + + pm_sincos(0.5 * mag, &sh, &(q->s)); + + if (q->s >= 0) { + q->x = uvec.x * sh; + q->y = uvec.y * sh; + q->z = uvec.z * sh; + } else { + q->s = -q->s; + q->x = -uvec.x * sh; + q->y = -uvec.y * sh; + q->z = -uvec.z * sh; + } + + return GO_RESULT_OK; +} + +int go_rvec_mat_convert(const go_rvec * r, go_mat * m) +{ + go_cart vec; + go_cart uvec; + go_real s, c, omc; + go_real mag; + + vec.x = r->x; + vec.y = r->y; + vec.z = r->z; + + if (GO_RESULT_OK != go_cart_unit(&vec, &uvec)) { + /* a zero vector */ + m->x.x = 1, m->y.x = 0, m->z.x = 0; + m->x.y = 0, m->y.y = 1, m->z.y = 0; + m->x.z = 0, m->y.z = 0, m->z.z = 1; + return GO_RESULT_OK; + } + + (void) go_cart_mag(&vec, &mag); + + pm_sincos(mag, &s, &c); + omc = 1 - c; + + m->x.x = c + go_sq(uvec.x) * omc; + m->y.x = -uvec.z * s + uvec.x * uvec.y * omc; + m->z.x = uvec.y * s + uvec.x * uvec.z * omc; + + m->x.y = uvec.z * s + uvec.y * uvec.x * omc; + m->y.y = c + go_sq(uvec.y) * omc; + m->z.y = -uvec.x * s + uvec.y * uvec.z * omc; + + m->x.z = -uvec.y * s + uvec.z * uvec.x * omc; + m->y.z = uvec.x * s + uvec.z * uvec.y * omc; + m->z.z = c + go_sq(uvec.z) * omc; + + return GO_RESULT_OK; +} + +int go_rvec_zyz_convert(const go_rvec * rvec, go_zyz * zyz) +{ + go_mat mat; + int retval; + + retval = go_rvec_mat_convert(rvec, &mat); + if (GO_RESULT_OK != retval) return retval; + + return go_mat_zyz_convert(&mat, zyz); +} + +int go_rvec_zyx_convert(const go_rvec * rvec, go_zyx * zyx) +{ + go_mat mat; + int retval; + + retval = go_rvec_mat_convert(rvec, &mat); + if (GO_RESULT_OK != retval) return retval; + + return go_mat_zyx_convert(&mat, zyx); +} + +int go_rvec_rpy_convert(const go_rvec * r, go_rpy * rpy) +{ + go_quat q; + int retval; + + retval = go_rvec_quat_convert(r, &q); + if (GO_RESULT_OK != retval) return retval; + + return go_quat_rpy_convert(&q, rpy); +} + +int go_quat_rvec_convert(const go_quat * q, go_rvec * r) +{ + go_real sh; + go_real mag; + + sh = sqrt(go_sq(q->x) + go_sq(q->y) + go_sq(q->z)); + + if (GO_ROT_SMALL(sh)) { + r->x = 0; + r->y = 0; + r->z = 0; + } else { + mag = 2 * atan2(sh, q->s) / sh; + r->x = mag * q->x; + r->y = mag * q->y; + r->z = mag * q->z; + } + + return GO_RESULT_OK; +} + +int go_quat_mat_convert(const go_quat * q, go_mat * m) +{ + /* from space book where e1=q->x e2=q->y e3=q->z e4=q->s */ + m->x.x = 1 - 2 * (go_sq(q->y) + go_sq(q->z)); + m->y.x = 2 * (q->x * q->y - q->z * q->s); + m->z.x = 2 * (q->z * q->x + q->y * q->s); + + m->x.y = 2 * (q->x * q->y + q->z * q->s); + m->y.y = 1 - 2 * (go_sq(q->z) + go_sq(q->x)); + m->z.y = 2 * (q->y * q->z - q->x * q->s); + + m->x.z = 2 * (q->z * q->x - q->y * q->s); + m->y.z = 2 * (q->y * q->z + q->x * q->s); + m->z.z = 1 - 2 * (go_sq(q->x) + go_sq(q->y)); + + return GO_RESULT_OK; +} + +int go_quat_zyz_convert(const go_quat * q, go_zyz * zyz) +{ + go_mat m; + int retval; + + retval = go_quat_mat_convert(q, &m); + if (GO_RESULT_OK != retval) return retval; + return go_mat_zyz_convert(&m, zyz); +} + +int go_quat_zyx_convert(const go_quat * q, go_zyx * zyx) +{ + go_mat m; + int retval; + + retval = go_quat_mat_convert(q, &m); + if (GO_RESULT_OK != retval) return retval; + return go_mat_zyx_convert(&m, zyx); +} + +int go_quat_rpy_convert(const go_quat * q, go_rpy * rpy) +{ + go_mat m; + int retval; + + retval = go_quat_mat_convert(q, &m); + if (GO_RESULT_OK != retval) return retval; + return go_mat_rpy_convert(&m, rpy); +} + +int go_mat_rvec_convert(const go_mat * m, go_rvec * r) +{ + go_quat q; + int retval; + + retval = go_mat_quat_convert(m, &q); + if (GO_RESULT_OK != retval) return retval; + + return go_quat_rvec_convert(&q, r); +} + +/* + from space book: + + e1 = (c32 - c23) / 4*e4 + e2 = (c13 - c31) / 4*e4 + e3 = (c21 - c12) / 4*e4 + e4 = sqrt(1 + c11 + c22 + c33) / 2 + + if e4 == 0 + e1 = sqrt(1 + c11 - c33 - c22) / 2 + e2 = sqrt(1 + c22 - c33 - c11) / 2 + e3 = sqrt(1 + c33 - c11 - c22) / 2 + + to determine whether to take the positive or negative sqrt value + since e4 == 0 indicates a 180* rotation then (0 x y z) = (0 -x -y -z). + Thus some generalities can be used: + 1) find which of e1, e2, or e3 has the largest magnitude and leave it pos + 2) if e1 is largest then + if c21 < 0 then take the negative for e2 + if c31 < 0 then take the negative for e3 + 3) else if e2 is largest then + if c21 < 0 then take the negative for e1 + if c32 < 0 then take the negative for e3 + 4) else if e3 is larger then + if c31 < 0 then take the negative for e1 + if c32 < 0 then take the negative for e2 + + Note: c21 in the space book is m.x.y in this C code +*/ +int go_mat_quat_convert(const go_mat * m, go_quat * q) +{ + go_real discr; + go_real a; + + if (! go_mat_is_norm(m)) { + return GO_RESULT_NORM_ERROR; + } + + discr = 1.0 + m->x.x + m->y.y + m->z.z; + if (discr < 0.0) discr = 0.0; /* give sqrt some slack for tiny negs */ + + q->s = 0.5 * sqrt(discr); + + if (GO_ROT_SMALL(q->s)) { + q->s = 0; + discr = 1.0 + m->x.x - m->y.y - m->z.z; + if (discr < 0.0) discr = 0.0; + q->x = sqrt(discr) / 2.0; + discr = 1.0 + m->y.y - m->x.x - m->z.z; + if (discr < 0.0) discr = 0.0; + q->y = sqrt(discr) / 2.0; + discr = 1.0 + m->z.z - m->y.y - m->x.x; + if (discr < 0.0) discr = 0.0; + q->z = sqrt(discr) / 2.0; + + if (q->x > q->y && q->x > q->z) { + if (m->x.y < 0.0) { + q->y *= -1; + } + if (m->x.z < 0.0) { + q->z *= -1; + } + } else if (q->y > q->z) { + if (m->x.y < 0.0) { + q->x *= -1; + } + if (m->y.z < 0.0) { + q->z *= -1; + } + } else { + if (m->x.z < 0.0) { + q->x *= -1; + } + if (m->y.z < 0.0) { + q->y *= -1; + } + } + } else { + q->x = (m->y.z - m->z.y) / (a = 4 * q->s); + q->y = (m->z.x - m->x.z) / a; + q->z = (m->x.y - m->y.x) / a; + } + + return go_quat_norm(q, q); +} + +int go_mat_zyz_convert(const go_mat * m, go_zyz * zyz) +{ + zyz->y = atan2(sqrt(go_sq(m->x.z) + go_sq(m->y.z)), m->z.z); + + if (GO_ROT_SMALL(zyz->y)) { + zyz->z = 0; + zyz->y = 0; /* force Y to 0 */ + zyz->zp = atan2(-m->y.x, m->x.x); + } else if (GO_ROT_CLOSE(zyz->y, GO_PI)) { + zyz->z = 0; + zyz->y = GO_PI; /* force Y to 180 */ + zyz->zp = atan2(m->y.x, -m->x.x); + } else { + zyz->z = atan2(m->z.y, m->z.x); + zyz->zp = atan2(m->y.z, -m->x.z); + } + + return GO_RESULT_OK; +} + +int go_mat_zyx_convert(const go_mat * m, go_zyx * zyx) +{ + zyx->y = atan2(-m->x.z, sqrt(go_sq(m->x.x) + go_sq(m->x.y))); + + if (GO_ROT_CLOSE(zyx->y, GO_PI_2)) { + zyx->z = 0; + zyx->y = GO_PI_2; /* force it */ + zyx->x = atan2(m->y.x, m->y.y); + } else if (GO_ROT_CLOSE(zyx->y, GO_PI_2)) { + zyx->z = 0; + zyx->y = -GO_PI_2; /* force it */ + zyx->x = -atan2(m->y.z, m->y.y); + } else { + zyx->z = atan2(m->x.y, m->x.x); + zyx->x = atan2(m->y.z, m->z.z); + } + + return GO_RESULT_OK; +} + +int go_mat_rpy_convert(const go_mat * m, go_rpy * rpy) +{ + rpy->p = atan2(-m->x.z, sqrt(go_sq(m->x.x) + go_sq(m->x.y))); + + if (GO_ROT_CLOSE(rpy->p, GO_PI_2)) { + rpy->r = atan2(m->y.x, m->y.y); + rpy->p = GO_PI_2; /* force it */ + rpy->y = 0; + } else if (GO_ROT_CLOSE(rpy->p, GO_PI_2)) { + rpy->r = -atan2(m->y.z, m->y.y); + rpy->p = -GO_PI_2; /* force it */ + rpy->y = 0; + } else { + rpy->r = atan2(m->y.z, m->z.z); + rpy->y = atan2(m->x.y, m->x.x); + } + + return GO_RESULT_OK; +} + +int go_zyz_rvec_convert(const go_zyz * zyz, go_rvec * r) +{ + go_mat m; + int retval; + + retval = go_zyz_mat_convert(zyz, &m); + if (GO_RESULT_OK != retval) return retval; + return go_mat_rvec_convert(&m, r); +} + +int go_zyz_quat_convert(const go_zyz * zyz, go_quat * q) +{ + go_mat m; + int retval; + + retval = go_zyz_mat_convert(zyz, &m); + if (GO_RESULT_OK != retval) return retval; + return go_mat_quat_convert(&m, q); +} + +int go_zyz_mat_convert(const go_zyz * zyz, go_mat * m) +{ + go_real sa, sb, sg; + go_real ca, cb, cg; + + sa = sin(zyz->z); + sb = sin(zyz->y); + sg = sin(zyz->zp); + + ca = cos(zyz->z); + cb = cos(zyz->y); + cg = cos(zyz->zp); + + m->x.x = ca * cb * cg - sa * sg; + m->y.x = -ca * cb * sg - sa * cg; + m->z.x = ca * sb; + + m->x.y = sa * cb * cg + ca * sg; + m->y.y = -sa * cb * sg + ca * cg; + m->z.y = sa * sb; + + m->x.z = -sb * cg; + m->y.z = sb * sg; + m->z.z = cb; + + return GO_RESULT_OK; +} + +int go_zyz_zyx_convert(const go_zyz * zyz, go_zyx * zyx) +{ + go_mat mat; + int retval; + + retval = go_zyz_mat_convert(zyz, &mat); + if (GO_RESULT_OK != retval) return retval; + return go_mat_zyx_convert(&mat, zyx); +} + +int go_zyz_rpy_convert(const go_zyz * zyz, go_rpy * rpy) +{ + go_mat mat; + int retval; + + retval = go_zyz_mat_convert(zyz, &mat); + if (GO_RESULT_OK != retval) return retval; + + return go_mat_rpy_convert(&mat, rpy); +} + +int go_zyx_rvec_convert(const go_zyx * zyx, go_rvec * r) +{ + go_mat mat; + int retval; + + retval = go_zyx_mat_convert(zyx, &mat); + if (GO_RESULT_OK != retval) return retval; + + return go_mat_rvec_convert(&mat, r); +} + + +int go_zyx_quat_convert(const go_zyx * zyx, go_quat * q) +{ + go_mat mat; + int retval; + + retval = go_zyx_mat_convert(zyx, &mat); + if (GO_RESULT_OK != retval) return retval; + + return go_mat_quat_convert(&mat, q); +} + +int go_zyx_mat_convert(const go_zyx * zyx, go_mat * m) +{ + go_real sa, sb, sg; + go_real ca, cb, cg; + + sa = sin(zyx->z); + sb = sin(zyx->y); + sg = sin(zyx->x); + + ca = cos(zyx->z); + cb = cos(zyx->y); + cg = cos(zyx->x); + + m->x.x = ca * cb; + m->y.x = ca * sb * sg - sa * cg; + m->z.x = ca * sb * cg + sa * sg; + + m->x.y = sa * cb; + m->y.y = sa * sb * sg + ca * cg; + m->z.y = sa * sb * cg - ca * sg; + + m->x.z = -sb; + m->y.z = cb * sg; + m->z.z = cb * cg; + + return GO_RESULT_OK; +} + +int go_zyx_zyz_convert(const go_zyx * zyx, go_zyz * zyz) +{ + go_mat mat; + int retval; + + retval = go_zyx_mat_convert(zyx, &mat); + if (GO_RESULT_OK != retval) return retval; + return go_mat_zyz_convert(&mat, zyz); +} + +int go_zyx_rpy_convert(const go_zyx * zyx, go_rpy * rpy) +{ + go_mat mat; + int retval; + + retval = go_zyx_mat_convert(zyx, &mat); + if (GO_RESULT_OK != retval) return retval; + return go_mat_rpy_convert(&mat, rpy); +} + +int go_rpy_rvec_convert(const go_rpy * rpy, go_rvec * rvec) +{ + go_quat quat; + int retval; + + retval = go_rpy_quat_convert(rpy, &quat); + if (GO_RESULT_OK != retval) return retval; + return go_quat_rvec_convert(&quat, rvec); +} + +int go_rpy_quat_convert(const go_rpy * rpy, go_quat * quat) +{ + go_mat mat; + int retval; + + retval = go_rpy_mat_convert(rpy, &mat); + if (GO_RESULT_OK != retval) return retval; + return go_mat_quat_convert(&mat, quat); +} + +int go_rpy_mat_convert(const go_rpy * rpy, go_mat * m) +{ + go_real sa, sb, sg; + go_real ca, cb, cg; + + sa = sin(rpy->y); + sb = sin(rpy->p); + sg = sin(rpy->r); + + ca = cos(rpy->y); + cb = cos(rpy->p); + cg = cos(rpy->r); + + m->x.x = ca * cb; + m->y.x = ca * sb * sg - sa * cg; + m->z.x = ca * sb * cg + sa * sg; + + m->x.y = sa * cb; + m->y.y = sa * sb * sg + ca * cg; + m->z.y = sa * sb * cg - ca * sg; + + m->x.z = -sb; + m->y.z = cb * sg; + m->z.z = cb * cg; + + return GO_RESULT_OK; +} + +int go_rpy_zyz_convert(const go_rpy * rpy, go_zyz * zyz) +{ + go_mat mat; + int retval; + + retval = go_rpy_mat_convert(rpy, &mat); + if (GO_RESULT_OK != retval) return retval; + return go_mat_zyz_convert(&mat, zyz); +} + +int go_rpy_zyx_convert(const go_rpy * rpy, go_zyx * zyx) +{ + go_mat mat; + int retval; + + retval = go_rpy_mat_convert(rpy, &mat); + if (GO_RESULT_OK != retval) return retval; + return go_mat_zyx_convert(&mat, zyx); +} + +go_pose go_pose_this(go_real x, go_real y, go_real z, + go_real rs, go_real rx, go_real ry, go_real rz) +{ + go_pose pose; + + pose.tran.x = x, pose.tran.y = y, pose.tran.z = z; + pose.rot.s = rs, pose.rot.x = rx, pose.rot.y = ry, pose.rot.z = rz; + + return pose; +} + +go_cart go_cart_zero(void) +{ + go_cart cart; + + cart.x = 0, cart.y = 0, cart.z = 0; + + return cart; +} + +go_quat go_quat_identity(void) +{ + go_quat quat; + + quat.s = 1, quat.x = 0, quat.y = 0, quat.z = 0; + + return quat; +} + +go_pose go_pose_identity(void) +{ + go_pose pose; + + pose.tran.x = 0, pose.tran.y = 0, pose.tran.z = 0; + pose.rot.s = 1, pose.rot.x = 0, pose.rot.y = 0, pose.rot.z = 0; + + return pose; +} + +int go_pose_hom_convert(const go_pose * p, go_hom * h) +{ + h->tran = p->tran; + + return go_quat_mat_convert(&p->rot, &h->rot); +} + +int go_hom_pose_convert(const go_hom * h, go_pose * p) +{ + p->tran = h->tran; + + return go_mat_quat_convert(&h->rot, &p->rot); +} + +int go_cart_rvec_convert(const go_cart * cart, go_rvec * rvec) +{ + rvec->x = cart->x; + rvec->y = cart->y; + rvec->z = cart->z; + + return GO_RESULT_OK; +} + +int go_rvec_cart_convert(const go_rvec * rvec, go_cart * cart) +{ + cart->x = rvec->x; + cart->y = rvec->y; + cart->z = rvec->z; + + return GO_RESULT_OK; +} + +/* go_cart functions */ + +go_flag go_cart_cart_compare(const go_cart * v1, const go_cart * v2) +{ + if (GO_TRAN_CLOSE(v1->x, v2->x) && + GO_TRAN_CLOSE(v1->y, v2->y) && + GO_TRAN_CLOSE(v1->z, v2->z)) { + return 1; + } + + return 0; +} + +int go_cart_cart_dot(const go_cart * v1, const go_cart * v2, + go_real * d) +{ + *d = v1->x * v2->x + v1->y * v2->y + v1->z * v2->z; + + return GO_RESULT_OK; +} + +int go_cart_cart_cross(const go_cart * v1, const go_cart * v2, + go_cart * vout) +{ + go_cart cp1, cp2; + + cp1 = *v1; + cp2 = *v2; + + vout->x = cp1.y * cp2.z - cp1.z * cp2.y; + vout->y = cp1.z * cp2.x - cp1.x * cp2.z; + vout->z = cp1.x * cp2.y - cp1.y * cp2.x; + + return GO_RESULT_OK; +} + +int go_cart_mag(const go_cart * v, go_real * d) +{ + *d = sqrt(go_sq(v->x) + go_sq(v->y) + go_sq(v->z)); + + return GO_RESULT_OK; +} + +int go_cart_magsq(const go_cart * v, go_real * d) +{ + *d = go_sq(v->x) + go_sq(v->y) + go_sq(v->z); + + return GO_RESULT_OK; +} + +go_flag go_cart_cart_par(const go_cart *v1, const go_cart *v2) +{ + go_real dot; + go_real magsq1, magsq2; + + /* + parallel if v1 dot v2 = mag v1 * mag v2, or more efficiently, + sq(v1 dot v2) = magsq v1 * magsq v2 + */ + + go_cart_cart_dot(v1, v2, &dot); + go_cart_magsq(v1, &magsq1); + go_cart_magsq(v2, &magsq2); + + return GO_TRAN_CLOSE(dot*dot, magsq1*magsq2); +} + +go_flag go_cart_cart_perp(const go_cart *v1, const go_cart *v2) +{ + go_real dot; + + /* perpendicular if v1 dot v2 is 0 */ + + go_cart_cart_dot(v1, v2, &dot); + + return GO_TRAN_SMALL(dot); +} + +int go_cart_cart_disp(const go_cart * v1, const go_cart * v2, + go_real * d) +{ + *d = sqrt(go_sq(v2->x - v1->x) + + go_sq(v2->y - v1->y) + + go_sq(v2->z - v1->z)); + + return GO_RESULT_OK; +} + +int go_cart_cart_add(const go_cart * v1, const go_cart * v2, + go_cart * vout) +{ + vout->x = v1->x + v2->x; + vout->y = v1->y + v2->y; + vout->z = v1->z + v2->z; + + return GO_RESULT_OK; +} + +int go_cart_cart_sub(const go_cart * v1, const go_cart * v2, + go_cart * vout) +{ + vout->x = v1->x - v2->x; + vout->y = v1->y - v2->y; + vout->z = v1->z - v2->z; + + return GO_RESULT_OK; +} + +int go_cart_scale_mult(const go_cart * v1, go_real d, go_cart * vout) +{ + vout->x = v1->x * d; + vout->y = v1->y * d; + vout->z = v1->z * d; + + return GO_RESULT_OK; +} + +int go_cart_neg(const go_cart * v1, go_cart * vout) +{ + vout->x = -v1->x; + vout->y = -v1->y; + vout->z = -v1->z; + + return GO_RESULT_OK; +} + +int go_cart_unit(const go_cart * v, go_cart * vout) +{ + go_real size = sqrt(go_sq(v->x) + go_sq(v->y) + go_sq(v->z)); + + if (GO_TRAN_SMALL(size)) { + vout->x = GO_INF; + vout->y = GO_INF; + vout->z = GO_INF; + + return GO_RESULT_NORM_ERROR; + } + + size = 1.0 / size; + + vout->x = v->x * size; + vout->y = v->y * size; + vout->z = v->z * size; + + return GO_RESULT_OK; +} + +int go_cart_is_norm(const go_cart * v) +{ + return GO_TRAN_CLOSE(sqrt(go_sq(v->x) + go_sq(v->y) + go_sq(v->z)), 1); +} + +int go_cart_cart_rot(const go_cart * v1, + const go_cart * v2, + go_quat * quat) +{ + int retval; + go_cart u1; + go_cart u2; + go_cart cross; + go_real mag; + go_real dot; + go_real th; + go_rvec rvec; + + /* unitize the input vectors */ + retval = go_cart_unit(v1, &u1); + if (GO_RESULT_OK != retval) return retval; + retval = go_cart_unit(v2, &u2); + if (GO_RESULT_OK != retval) return retval; + /* and cross the unit vectors to get the mutual normal */ + (void) go_cart_cart_cross(&u1, &u2, &cross); + /* the magnitude of the mutual normal is sin theta */ + (void) go_cart_mag(&cross, &mag); + th = asin(mag); + /* dot them to get parallel (1) or antiparallel (-1) */ + (void) go_cart_cart_dot(&u1, &u2, &dot); + /* make the cross a unit vector so we can theta-ize it */ + retval = go_cart_unit(&cross, &cross); + + /* handle aligned vectors */ + if (GO_RESULT_OK != retval) { + /* v1 and v2 are aligned, so th will be zero */ + if (dot > 0.0) { /* would be -1 or 1 */ + /* parallel */ + quat->s = 1.0; + quat->x = quat->y = quat->z = 0.0; + return GO_RESULT_OK; + } + /* else antiparallel - set u2 to be normal to u1, + rotate about it PI rads */ + retval = go_cart_normal(&u1, &u2); + if (GO_RESULT_OK != retval) return retval; + rvec.x = GO_PI * u2.x; + rvec.y = GO_PI * u2.y; + rvec.z = GO_PI * u2.z; + return go_rvec_quat_convert(&rvec, quat); + } + + /* else not aligned */ + if (dot < 0.0) { + th = GO_PI - th; + } + rvec.x = th * cross.x; + rvec.y = th * cross.y; + rvec.z = th * cross.z; + return go_rvec_quat_convert(&rvec, quat); +} + +int go_cart_cart_proj(const go_cart * v1, const go_cart * v2, + go_cart * vout) +{ + go_cart v2u; + go_real d; + int retval; + + retval = go_cart_unit(v2, &v2u); + if (GO_RESULT_OK != retval) return retval; + + retval = go_cart_cart_dot(v1, &v2u, &d); + if (GO_RESULT_OK != retval) return retval; + + return go_cart_scale_mult(&v2u, d, vout); +} + +int go_cart_plane_proj(const go_cart * v, const go_cart * normal, + go_cart * vout) +{ + go_cart par; + int retval; + + retval = go_cart_cart_proj(v, normal, &par); + if (GO_RESULT_OK != retval) return retval; + + return go_cart_cart_sub(v, &par, vout); +} + +int go_cart_cart_angle(const go_cart * v1, const go_cart * v2, + go_real * a) +{ + go_real dot, m1, m2; + + go_cart_cart_dot(v1, v2, &dot); + go_cart_mag(v1, &m1); + go_cart_mag(v2, &m2); + + if (m1 <= 0. || m2 <= 0.) { + return GO_RESULT_DIV_ERROR; + } + + dot = dot / (m1 * m2); + if (dot > 1.) + dot = 1.; + else if (dot < -1.) + dot = -1.; + + *a = acos(dot); + + return GO_RESULT_OK; +} + +int go_cart_normal(const go_cart * v, go_cart * vout) +{ + go_cart cart; + go_real min, ymin; + + /* pick the X, Y or Z vector that is most perpendicular to 'v' */ + cart.x = 1, cart.y = 0, cart.z = 0; /* start with X */ + min = fabs(v->x); + ymin = fabs(v->y); + if (ymin < min) { + min = ymin; /* Y is more perp */ + cart.x = 0, cart.y = 1, cart.z = 0; + } + if (fabs(v->z) < min) { + cart.x = 0, cart.y = 0, cart.z = 1; + } + + /* cross the most perp axis vector with 'v' to get a real perp */ + go_cart_cart_cross(v, &cart, &cart); + + /* unitize it into 'vout' and return */ + return go_cart_unit(&cart, vout); +} + +int go_cart_centroid(const go_cart * varray, + go_integer num, + go_cart * centroid) +{ + go_integer i; + + if (num < 1) return GO_RESULT_BAD_ARGS; + + centroid->x = varray[0].x; + centroid->y = varray[0].y; + centroid->z = varray[0].z; + + for (i = 1; i < num; i++) { + centroid->x += varray[i].x; + centroid->y += varray[i].y; + centroid->z += varray[i].z; + } + + return go_cart_scale_mult(centroid, 1.0/((go_real) num), centroid); +} + +int go_cart_centroidize(const go_cart * vinarray, + go_integer num, + go_cart * centroid, + go_cart * voutarray) +{ + go_integer i; + int retval; + + retval = go_cart_centroid(vinarray, num, centroid); + if (GO_RESULT_OK != retval) return retval; + + for (i = 0; i < num; i++) { + (void) go_cart_cart_sub(&vinarray[i], centroid, &voutarray[i]); + } + + return GO_RESULT_OK; +} + +/* + Another Solution of the Cubic Equation Here is another method of + solving a cubic polynomial equation submitted independently by Paul + A. Torres and Robert A. Warren. It is based on the idea of "completing + the cube," by arranging matters so that three of the four terms are + three of the four terms of a perfect cube. + + Start with the cubic equation + + x3 + ax2 + bx + c = 0. + + If a2 - 3b = 0, then the first three terms are the first three terms + of a perfect cube, namely (x+a/3)3. Then you can "complete the cube" + by subtracting c from both sides and adding the missing term of the + cube a3/27 to both sides. Recalling that b = a2/3, you get + + x3 + ax2 + bx + c = 0, + x3 + ax2 + (a2/3)x + c = 0, + x3 + ax2 + (a2/3)x = -c, + x3 + ax2 + (a2/3)x + a3/27 = a3/27 - c, + (x+a/3)3 = a3/27 - c. + + By taking the cube root of the left side and the three cube roots of + the right side, you get + + x = -a/3 + (a3/27 - c)1/3, + x = -a/3 + (a3/27 - c)1/3(-1+sqrt[-3])/2, + x = -a/3 + (a3/27 - c)1/3(-1-sqrt[-3])/2. + + These are the roots of the cubic equation that were sought. + + If a2 - 3b is nonzero, then proceed as follows. Set x = y + z, where y + is an indeterminate and z is a function of a, b, and c, which will be + found below. Then + + (y+z)3 + a(y+z)2 + b(y+z) + c = 0, + y3 + (3z+a)y2 + (3z2+2az+b)y + (z3+az2+bz+c) = 0, + y3 + dy2 + ey + f = 0, + + where + + d = 3z + a, + e = 3z2 + 2az + b, + f = z3 + az2 + bz + c. + + The first three terms of this equation in y will be those of a perfect + cube if and only if d2 - 3e is zero, which happens if and only if a2 - + 3b = 0, which cannot happen in this case, so we seemingly haven't + gained anything. However, the last three terms of this equation in y + will be those of a perfect cube if and only if e2 = 3df, that is if + and only if + + (3z2+2az+b)2 = 3(3z+a)(z3+az2+bz+c), + (a2-3b)z2 + (ab-9c)z + (b2-3ac) = 0, + gz2 + hz + i = 0, + + where + + g = a2 - 3b, + h = ab - 9c, + i = b2 - 3ac. + + Since a2 - 3b is nonzero, g is nonzero, and we have a true quadratic + equation, called the resolvent quadratic. Now we pick z to be a root + of this quadratic equation. If the GCD of z2 + (h/g)z + (i/g) and z3 + + az2 + bz + c as polynomials in z is not 1, then any root of the GCD is + also a root of the original cubic equation in x. Once you have at + least one root, the problem of finding the other roots is reduced to + solving a quadratic or linear equation. If the GCD is 1, then neither + value of z can make f = 0, so we can assume henceforth that f is + nonzero. Either root z of the quadratic will do, but we must choose + one of them. We arbitrarily pick the one with a plus sign in front of + the radical, + + z = (-h+sqrt[h2-4gi])/(2g), + = (9c-ab+sqrt[81c2-54abc+12a3c+12b3-3a2b2])/(2[a2-3b]). + + Set z equal to this value in the equation for y, and divide it by f on + both sides. Then the last three terms of the cubic in y are those of a + perfect cube, namely (ey/[3f]+1)3, so we can complete the cube to + solve it. We do this by subtracting y3/f from both sides, then adding + the missing term of the cubic, (ey/[3f])3 to both sides, obtaining + + (ey/[3f]+1)3 = ([e/(3f)]3-1/f)y3, + + ey/(3f) + 1 = y([e/(3f)]3-1/f)1/3, + ey/(3f) + 1 = y([e/(3f)]3-1/f)1/3(-1+sqrt[-3])/2, + ey/(3f) + 1 = y([e/(3f)]3-1/f)1/3(-1-sqrt[-3])/2, + + y = 3f/[-e+(e3-27f2)1/3], + y = 3f/[-e+(e3-27f2)1/3(-1+sqrt[-3])/2], + y = 3f/[-e+(e3-27f2)1/3(-1-sqrt[-3])/2]. + + Now you have the values of y. Add z to each to get the values of x: + + x = z + 3f/[(e3-27f2)1/3-e], + x = z + 3f/[(e3-27f2)1/3(-1+sqrt[-3])/2-e], + x = z + 3f/[(e3-27f2)1/3(-1-sqrt[-3])/2-e]. + + These are the roots of the cubic equation that were sought. + + Example: Solve x3 + 6x2 + 9x + 6 = 0. + + a = 6, b = 9, c = 6. + + Then + + d = 3z + 6, e = 3z2 + 12z + 9, f = z3 + 6z2 + 9z + 6. + + Then + + g = a2 - 3b = 36 - 27 = 9, + h = ab - 9c = 54 - 54 = 0, + i = b2 - 3ac = 81 - 108 = -27, + + and the resolvent quadratic is + + 9z2 - 27 = 0, + z2 - 3 = 0, + z = sqrt(3). + + Then + + d = 6 + 3 sqrt(3), + e = 18 + 12 sqrt(3), + f = 24 + 12 sqrt(3), + + and the cubic in y is + + y3 + (6+3 sqrt[3])y2 + (18+12 sqrt[3])y + (24+12 sqrt[3]) = 0. + + Then one root is + + y = 3f/[(e3-27f2)1/3-e], + = 3(24+12 sqrt[3])/([18+12 sqrt(3)]3-27[24+12 sqrt(3)]2)1/3-18-12 sqrt[3]), + = (12+6 sqrt[3])/([9+6 sqrt(3)]1/3-3-2 sqrt[3]), + = (6+4 sqrt[3])/([2+sqrt(3)]1/3-2-sqrt[3]). + + After a lot of simplification, you get + + y = -2 - sqrt(3) - (2-sqrt[3])1/3 - (2+sqrt[3])1/3, + x = -2 - (2-sqrt[3])1/3 - (2+sqrt[3])1/3 + + (and two other roots). +*/ + +go_complex go_complex_add(go_complex z1, go_complex z2) +{ + go_complex sum; + + sum.re = z1.re + z2.re; + sum.im = z1.im + z2.im; + + return sum; +} + +go_complex go_complex_sub(go_complex z1, go_complex z2) +{ + go_complex diff; + + diff.re = z1.re - z2.re; + diff.im = z1.im - z2.im; + + return diff; +} + +go_complex go_complex_mult(go_complex z1, go_complex z2) +{ + go_complex prod; + + prod.re = (z1.re * z2.re) - (z1.im * z2.im); + prod.im = (z1.re * z2.im) + (z1.im * z2.re); + + return prod; +} + +go_complex go_complex_div(go_complex z1, go_complex z2, int * result) +{ + go_complex z2c = {0, 0}; + go_real denom; + + denom = go_sq(z2.re) + go_sq(z2.im); + if (denom < GO_REAL_EPSILON) { + *result = GO_RESULT_DIV_ERROR; + return z2c; + } + + z2c.re = z2.re, z2c.im = -z2.im; /* complex conjugate */ + *result = GO_RESULT_OK; + return go_complex_scale(go_complex_mult(z1, z2c), 1.0 / denom); +} + +go_complex go_complex_scale(go_complex z, go_real scale) +{ + go_complex s; + + s.re = scale * z.re; + s.im = scale * z.im; + + return s; +} + +go_real go_complex_mag(go_complex z) +{ + return sqrt(go_sq(z.re) + go_sq(z.im)); +} + +go_real go_complex_arg(go_complex z) +{ + return atan2(z.im, z.re); +} + +/* + go_complex_sqrt() and go_complex_cbrt() are structured so that if + the roots of complex conjugates are taken, the first roots z1 will + be complex conjugates. This is due to atan2 returning args in the + range -PI to PI. + */ + +void go_complex_sqrt(go_complex z, go_complex * z1, go_complex * z2) +{ + go_real r, th; + + r = sqrt(go_complex_mag(z)); + th = 0.5 * go_complex_arg(z); + + z1->re = r * cos(th); + z1->im = r * sin(th); + if (NULL != z2) { + z2->re = -z1->re; + z2->im = -z1->im; + } + + return; +} + +void go_complex_cbrt(go_complex z, go_complex * z1, go_complex * z2, go_complex * z3) +{ + go_real r, th; + + r = go_cbrt(go_complex_mag(z)); + th = 1.0/3.0 * go_complex_arg(z); + + z1->re = r * cos(th); + z1->im = r * sin(th); + if (NULL != z2) { + z2->re = r * cos(th + 2.0/3.0 * GO_PI); + z2->im = r * sin(th + 2.0/3.0 * GO_PI); + } + if (NULL != z3) { + z3->re = r * cos(th + 4.0/3.0 * GO_PI); + z3->im = r * sin(th + 4.0/3.0 * GO_PI); + } + + return; +} + +int go_quadratic_solve(const go_quadratic * quad, + go_complex * z1, + go_complex * z2) +{ + go_real discr; + + /* check if b is 0 and we can reduce order */ + if (GO_SMALL(quad->b)) { + z2->re = z2->im = 0.0; + z1->re = -quad->a; + z1->im = 0.0; + return GO_RESULT_OK; + } + + discr = go_sq(quad->a) - 4.0 * quad->b; + if (discr < 0.0) { + discr = sqrt(-discr); + z1->re = z2->re = -0.5 * quad->a; + z1->im = 0.5 * discr; + z2->im = -0.5 * discr; + } else { + discr = sqrt(discr); + z1->re = 0.5 * (-quad->a + discr); + z2->re = 0.5 * (-quad->a - discr); + z1->im = z2->im = 0.0; + } + + return GO_RESULT_OK; +} + +/* + References on solving the cubic: + + "Solving Cubic and Quartic Polynomials," + www.karlscalculus.org/pdf/cubicQuartic.pdf + + "Cubic Equations in One Formula," + http://mathforum.org/library/drmath/view/52668.html +*/ +int go_cubic_solve(const go_cubic * cub, + go_complex * z1, + go_complex * z2, + go_complex * z3) +{ + go_quadratic quad; + go_real a, b; + go_complex z, A, B, I = {0.0, 1.0}; + go_complex u1, u2, u3; + + /* check if c is 0, and we have a root of 0 and can reduce order + to a quadratic */ + if (GO_SMALL(cub->c)) { + z3->re = z3->im = 0.0; + quad.a = cub->a; + quad.b = cub->b; + return go_quadratic_solve(&quad, z1, z2); + } + + a = cub->b - 1.0/3.0 * go_sq(cub->a); + b = cub->c - 1.0/3.0 * cub->a * cub->b + 2.0/27.0 * go_cub(cub->a); + z.re = 0.25 * go_sq(b) + 1.0/27.0 * go_cub(a), z.im = 0.0; + go_complex_sqrt(z, &A, &B); + + /* since z is real, the pair of sqrt(z) is either pure real or + pure complex */ + A.re -= 0.5 * b; + B.re -= 0.5 * b; + /* subtracting b/2 keeps them pure real and in general unequal, or + makes them complex conjugates */ + + /* Now we need to take the cube root of each. If they're pure real, + we can take the scalar cube root. */ + if (GO_SMALL(A.im)) { + A.re = go_cbrt(A.re), A.im = 0.0; + B.re = go_cbrt(B.re), B.im = 0.0; + } else { + go_complex_cbrt(A, &A, NULL, NULL); + B.re = A.re, B.im = -A.im; /* B and A must be complex conjugates */ + } + u1 = go_complex_add(A, B); + u2 = u3 = go_complex_scale(u1, -0.5); + z = go_complex_scale(go_complex_sub(A, B), sqrt(0.75)); + z = go_complex_mult(I, z); + u2 = go_complex_add(u2, z); + u3 = go_complex_sub(u3, z); + + z1->re = u1.re - 1.0/3.0 * cub->a, z1->im = u1.im; + z2->re = u2.re - 1.0/3.0 * cub->a, z2->im = u2.im; + z3->re = u3.re - 1.0/3.0 * cub->a, z3->im = u3.im; + + return GO_RESULT_OK; +} + +/* + http://mathforum.org/dr.math/faq/faq.cubic.equations.html + + Solving the quartic with no third-order term: + + The quartic in y must factor into two quadratics with real + coefficients, since any complex roots must occur in conjugate + pairs. Write + + y4 + e y2 + f y + g = (y2 + h y + j) (y2 - h y + g/j) + + Since f and g aren't zero, neither j nor h is zero either. Without + loss of generality, we can assume h > 0 (otherwise swap the two + factors). Then, equating coefficients of y2 and y, + + e = g/j + j - h2, f = h (g/j - j). + + so + + g/j + j = e + h2, g/j - j = f/h. + + Adding and subtracting these two equations, + + 2 g/j = e + h2 + f/h, 2 j = e + h2 - f/h. + + Multiplying these together, + + 4 g = e2 + 2 e h2 + h4 - f2/h2. + + Rearranging, + + h6 + 2 e h4 + (e2-4 g) h2 - f2 = 0. + + This is a cubic equation in h2, with known coefficients, since we know + e, f, and g. We can use the solution for cubic equations shown above + to find a positive real value of h2, whose existence is guaranteed by + Descartes' Rule of Signs, and then take its positive square root. This + value of h will give a value of j from 2 j = e + h2 - f/h. Once you + know h and j, you know the quadratic factors of the quartic in + y. Notice that you do not need all the roots h2 of the cubic, and that + any positive real one will do. Further notice that h2 = 4 z from the + previous section. + + Once you have factored the quartic into two quadratics, finishing the + finding of the roots is simple, using the Quadratic Formula. Once the + roots y are found, the corresponding x's are gotten from x = y - a/4. +*/ + +int go_quartic_solve(const go_quartic * quart, + go_complex * z1, + go_complex * z2, + go_complex * z3, + go_complex * z4) +{ + go_cubic cub; + go_quadratic quad; + go_real a2, a3, a4; + go_real e, f, g; + go_complex p, q, r, fc, a4c; + int retval; + + /* + There are several ways to solve quartic equations. They all start by + dividing the equation by the leading coefficient, to make it of the + form + + x4 + a x3 + b x2 + c x + d = 0. + + There is an easy special case if d = 0, in which case you can factor + the quartic into x times a cubic. The roots then are 0 and the roots + of that cubic. (To solve the cubic equation, see above.) + */ + if (GO_SMALL(quart->d)) { + /* set one root to be 0 and solve for reduced cubic */ + z4->re = z4->im = 0.0; + cub.a = quart->a; + cub.b = quart->b; + cub.c = quart->c; + return go_cubic_solve(&cub, z1, z2, z3); + } + + /* + First eliminate the third-order term. Substitute x = y - a/4, + expand, and simplify, to get + + y4 + e y2 + f y + g = 0 + + where + + e = b - 3 a2/8, + f = c + a3/8 - a b/2, + g = d - 3 a4/256 + a2 b/16 - a c/4. + */ + a2 = go_sq(quart->a); + e = quart->b - 3.0/8.0 * a2; + a3 = a2 * quart->a; + f = quart->c + 1.0/8.0 * a3 - 0.5 * (quart->a * quart->b); + a4 = a2 * a2; + g = quart->d - 3.0/256.0 * a4 + 1.0/16.0 * a2 * quart->b - 0.25 * (quart->a * quart->c); + /* + At this point, there are two useful special cases. + + 1. If g = 0, then, again, you can factor the quartic into y times + a cubic. The roots of the original equation are then x = -a/4 + and the roots of that cubic with a/4 subtracted from each. + + 2. If f = 0, then the quartic in y is actually a quadratic + equation in the variable y2. Solve this using your favorite + method, and then take the two square roots of each of the + solutions for y2 to find the four values of y which + work. Subtract a/4 from each to get the four roots x. + + Henceforth we can assume that d, f, and g are all nonzero. + */ + /* Case 1 */ + if (GO_SMALL(g)) { + a4 = -0.25 * quart->a; /* reuse a4 as -a/4, save a var */ + z4->re = a4; + z4->im = 0.0; + cub.a = 0.0; + cub.b = e; + cub.c = f; + retval = go_cubic_solve(&cub, z1, z2, z3); + if (GO_RESULT_OK != retval) return retval; + z1->re += a4; + z2->re += a4; + z3->re += a4; + return GO_RESULT_OK; + } + /* Case 2 */ + if (GO_SMALL(f)) { + a4 = -0.25 * quart->a; /* reuse a4 as -a/4, save a var */ + quad.a = e; + quad.b = g; + retval = go_quadratic_solve(&quad, z1, z3); + if (GO_RESULT_OK != retval) return retval; + go_complex_sqrt(*z1, z1, z2); + go_complex_sqrt(*z3, z3, z4); + z1->re += a4; + z2->re += a4; + z3->re += a4; + z4->re += a4; + return GO_RESULT_OK; + } + + /* + Here is one way to proceed from this point. The related auxiliary + cubic equation + + z3 + (e/2) z2 + ((e2-4 g)/16) z - f2/64 = 0 + + has three roots. Find those roots by solving this cubic (again, + see above). None of these is zero because f isn't zero. Let p and + q be the square roots of two of those roots (any choices of roots + and signs will work), and set + + r = -f/(8 p q). + + Then p2, q2, and r2 are the three roots of the above cubic. More + important is the fact that the four roots of the original quartic + are + + x = p + q + r - a/4, + x = p - q - r - a/4, + x = -p + q - r - a/4, and + x = -p - q + r - a/4. + + This solution was discovered by Leonhard Euler (1707-1783). + */ + cub.a = 0.5 * e; + cub.b = 1.0/16.0 * (go_sq(e) - 4.0 * g); + cub.c = -1.0/64.0 * go_sq(f); + retval = go_cubic_solve(&cub, z1, z2, z3); + if (GO_RESULT_OK != retval) return retval; + go_complex_sqrt(*z1, &p, NULL); + go_complex_sqrt(*z2, &q, NULL); + fc.re = f, fc.im = 0.0; /* fc is complex of scalar f */ + r = go_complex_div(fc, go_complex_scale(go_complex_mult(p, q), -8.0), &retval); + if (GO_RESULT_OK != retval) return retval; + + a4c.re = 0.25 * quart->a, a4c.im = 0.0; /* a/4 as a complex */ + *z1 = go_complex_sub(go_complex_add(go_complex_add(p, q), r), a4c); + *z2 = go_complex_sub(go_complex_sub(go_complex_sub(p, q), r), a4c); + *z3 = go_complex_sub(go_complex_sub(go_complex_sub(q, p), r), a4c); + *z4 = go_complex_sub(go_complex_sub(go_complex_sub(r, p), q), a4c); + + return GO_RESULT_OK; +} + +#define SIGN(a,b) ((b) >= 0.0 ? fabs(a) : -fabs(a)) +#define MAG2(a,b) sqrt((a)*(a)+(b)*(b)) + +/*! + This computes the Householder reduction to tridiagonal form of a + real symmetric matrix \a a. \a a will contain the orthogonal matrix + that effects the transformation. \a d will contain the diagonal + elements, and \a e will contain the off-diagonal elements. This is + used by go_tridiag_ql to find the eigenvalues and eigenvectors of a + real symmetric matrix. + */ +int go_tridiag_reduce(go_real ** a, /*< the real symmetric matrix, + overwritten with the diagonalizing + matrix */ + go_integer n, /*< how many rows and columns */ + go_real * d, /*< the diagonal elements will be + stored here */ + go_real * e) /*< the off-diagonal elements + will be stored here, starting + at index 1 */ +{ + go_integer l,k,j,i; + go_real scale,hh,h,g,f; + + for (i=n-1;i>0;i--) { + l=i-1; + h=scale=0.0; + if (l > 0) { + for (k=0;k<=l;k++) + scale += fabs(a[i][k]); + if (scale == 0.0) + e[i]=a[i][l]; + else { + for (k=0;k<=l;k++) { + a[i][k] /= scale; + h += a[i][k]*a[i][k]; + } + f=a[i][l]; + g=(f >= 0.0 ? -sqrt(h) : sqrt(h)); + e[i]=scale*g; + h -= f*g; + a[i][l]=f-g; + f=0.0; + for (j=0;j<=l;j++) { + a[j][i]=a[i][j]/h; + g=0.0; + for (k=0;k<=j;k++) + g += a[j][k]*a[i][k]; + for (k=j+1;k<=l;k++) + g += a[k][j]*a[i][k]; + e[j]=g/h; + f += e[j]*a[i][j]; + } + hh=f/(h+h); + for (j=0;j<=l;j++) { + f=a[i][j]; + e[j]=g=e[j]-hh*f; + for (k=0;k<=j;k++) + a[j][k] -= (f*e[k]+g*a[i][k]); + } + } + } else + e[i]=a[i][l]; + d[i]=h; + } + d[0]=0.0; + e[0]=0.0; + for (i=0;i=l;i--) { + f=s*e[i]; + b=c*e[i]; + e[i+1]=(r=MAG2(f,g)); + if (r == 0.0) { + d[i+1] -= p; + e[m]=0.0; + break; + } + s=f/r; + c=g/r; + g=d[i+1]-p; + r=(d[i]-g)*s+2.0*c*b; + d[i+1]=g+(p=s*r); + g=c*r-b; + for (k=0;k= l) continue; + d[l] -= p; + e[l]=g; + e[m]=0.0; + } + } while (m != l); + } + + return GO_RESULT_OK; +} + +int go_cart_cart_pose(const go_cart * v1, const go_cart * v2, + go_cart * v1c, go_cart * v2c, + go_integer num, go_pose * p) +{ + go_integer t; + go_cart c1, c2; + go_real Sxx, Sxy, Sxz; + go_real Syx, Syy, Syz; + go_real Szx, Szy, Szz; + GO_MATRIX_DECLARE(N, Nspace, 4, 4); + go_real d[4], e[4]; + go_real eigenval; + int retval; + + memset(&Nspace,0,sizeof(Nspace)); + + Sxx = Sxy = Sxz = 0.0; + Syx = Syy = Syz = 0.0; + Szx = Szy = Szz = 0.0; + + go_matrix_init(N, Nspace, 4, 4); + + retval = go_cart_centroidize(v1, num, &c1, v1c); + if (GO_RESULT_OK != retval) return retval; + retval = go_cart_centroidize(v2, num, &c2, v2c); + if (GO_RESULT_OK != retval) return retval; + + for (t = 0; t < num; t++) { + Sxx += v1c[t].x * v2c[t].x; + Sxy += v1c[t].x * v2c[t].y; + Sxz += v1c[t].x * v2c[t].z; + + Syx += v1c[t].y * v2c[t].x; + Syy += v1c[t].y * v2c[t].y; + Syz += v1c[t].y * v2c[t].z; + + Szx += v1c[t].z * v2c[t].x; + Szy += v1c[t].z * v2c[t].y; + Szz += v1c[t].z * v2c[t].z; + } + + N.el[0][0] = Sxx + Syy + Szz; + N.el[0][1] = N.el[1][0] = Syz - Szy; + N.el[0][2] = N.el[2][0] = Szx - Sxz; + N.el[0][3] = N.el[3][0] = Sxy - Syx; + + N.el[1][1] = Sxx - Syy - Szz; + N.el[1][2] = N.el[2][1] = Sxy + Syx; + N.el[1][3] = N.el[3][1] = Szx + Sxz; + + N.el[2][2] = -Sxx + Syy - Szz; + N.el[2][3] = N.el[3][2] = Syz + Szy; + + N.el[3][3] = -Sxx -Syy + Szz; + + /* compute eigenvectors */ + retval = go_tridiag_reduce(N.el, 4, d, e); + if (GO_RESULT_OK != retval) return retval; + retval = go_tridiag_ql(d, e, 4, N.el); + if (GO_RESULT_OK != retval) return retval; + + /* pick eigenvector associated with most positive eigenvalue */ + eigenval = d[0], t = 0; + if (d[1] > eigenval) eigenval = d[1], t = 1; + if (d[2] > eigenval) eigenval = d[2], t = 2; + if (d[3] > eigenval) eigenval = d[3], t = 3; + /* now t is the column with the eigenvector we want as our quaternion */ + p->rot.s = N.el[0][t]; + p->rot.x = N.el[1][t]; + p->rot.y = N.el[2][t]; + p->rot.z = N.el[3][t]; + retval = go_quat_norm(&p->rot, &p->rot); + if (GO_RESULT_OK != retval) return retval; + + /* rotate left centroid, subtract right from left to get translation */ + (void) go_quat_cart_mult(&p->rot, &c1, &c1); + (void) go_cart_cart_sub(&c2, &c1, &p->tran); + + return GO_RESULT_OK; +} + +static int trilaterate(go_real x2, go_real x3, go_real y3, + go_real l1, go_real l2, go_real l3, + go_cart * p) +{ + go_real discr; + + p->x = 0.5 * (x2 - (go_sq(l2) - go_sq(l1))/x2); + + p->y = + (go_sq(l1) - 2.0 * p->x * x3 + go_sq(x3) + + go_sq(y3) - go_sq(l3)) / (2.0 * y3); + + discr = go_sq(l1) - go_sq(p->x) - go_sq(p->y); + if (discr < -GO_REAL_EPSILON) { + return GO_RESULT_DOMAIN_ERROR; + } + if (discr < 0.0) discr = 0.0; /* make slightly negative numbers 0 */ + + /* return z positive, caller can make negative if they want the + point below the base */ + p->z = sqrt(discr); + + return GO_RESULT_OK; +} + +int go_cart_trilaterate(const go_cart * c1, + const go_cart * c2, + const go_cart * c3, + go_real l1, + go_real l2, + go_real l3, + go_cart * out1, + go_cart * out2) +{ + go_pose P_to_B, B_to_P; + go_mat mat; + go_cart diff, proj; + go_cart p1, p2, p3, out1_in_P, out2_in_P; + int retval; + + /* transform the points from their original base frame {B} into an + easier frame {P}, with c1 at the {P}, origin, c2 along the {P} x + axis, and all three in the {P} xy plane */ + + /* first get the transform */ + P_to_B.tran = *c1; + go_cart_cart_sub(c2, c1, &diff); + retval = go_cart_unit(&diff, &mat.x); + if (GO_RESULT_OK != retval) return retval; + go_cart_cart_sub(c3, c1, &diff); + retval = go_cart_cart_proj(&diff, &mat.x, &proj); + if (GO_RESULT_OK != retval) return retval; + go_cart_cart_sub(&diff, &proj, &diff); + retval = go_cart_unit(&diff, &mat.y); + if (GO_RESULT_OK != retval) return retval; + go_cart_cart_cross(&mat.x, &mat.y, &mat.z); + retval = go_mat_quat_convert(&mat, &P_to_B.rot); + if (GO_RESULT_OK != retval) return retval; + retval = go_pose_inv(&P_to_B, &B_to_P); + if (GO_RESULT_OK != retval) return retval; + /* now transform them */ + go_pose_cart_mult(&B_to_P, c1, &p1); + go_pose_cart_mult(&B_to_P, c2, &p2); + go_pose_cart_mult(&B_to_P, c3, &p3); + + /* trilaterate to get the point in {P} */ + retval = trilaterate(p2.x, p3.x, p3.y, l1, l2, l3, &out1_in_P); + if (GO_RESULT_OK != retval) return retval; + out2_in_P.x = out1_in_P.x; + out2_in_P.y = out1_in_P.y; + out2_in_P.z = -out1_in_P.z; /* the other one has a minus z */ + go_pose_cart_mult(&P_to_B, &out1_in_P, out1); + go_pose_cart_mult(&P_to_B, &out2_in_P, out2); + + return GO_RESULT_OK; +} + +/* go_rvec functions */ + +go_flag go_rvec_rvec_compare(const go_rvec * r1, const go_rvec * r2) +{ + if (GO_ROT_CLOSE(r1->x, r2->x) && + GO_ROT_CLOSE(r1->y, r2->y) && + GO_ROT_CLOSE(r1->z, r2->z)) { + return 1; + } + + return 0; +} + +int go_rvec_scale_mult(const go_rvec * r, go_real s, go_rvec * rout) +{ + rout->x = r->x * s; + rout->y = r->y * s; + rout->z = r->z * s; + + return GO_RESULT_OK; +} + +/* go_mat functions */ + +int go_mat_norm(const go_mat * mat, go_mat * mout) +{ + go_cart yprojx; + int retval; + + /* unitize the X vector, which we must be able to do */ + retval = go_cart_unit(&mat->x, &mout->x); + if (GO_RESULT_OK != retval) return retval; + + /* project Y onto X, subtract from Y to get Y perp to X, and unitize */ + retval = go_cart_cart_proj(&mat->y, &mout->x, &yprojx); + if (GO_RESULT_OK != retval) return retval; + go_cart_cart_sub(&mat->y, &yprojx, &mout->y); + retval = go_cart_unit(&mout->y, &mout->y); + if (GO_RESULT_OK != retval) return retval; + + /* cross the new X and Y to get Z */ + return go_cart_cart_cross(&mout->x, &mout->y, &mout->z); +} + +go_flag go_mat_is_norm(const go_mat * m) +{ + go_cart u; + + go_cart_cart_cross(&m->x, &m->y, &u); + +#define COL_IS_UNIT(r) GO_TRAN_CLOSE(go_sq((r).x) + go_sq((r).y) + go_sq((r).z), 1) + return (COL_IS_UNIT(m->x) && + COL_IS_UNIT(m->y) && + COL_IS_UNIT(m->z) && + go_cart_cart_compare(&u, &m->z)); +#undef COL_IS_UNIT +} + +int go_mat_inv(const go_mat * m, go_mat * mout) +{ + go_real cp; + + /* inverse of a rotation matrix is the transpose */ + + mout->x.x = m->x.x; + mout->y.y = m->y.y; + mout->z.z = m->z.z; + + cp = m->x.y; + mout->x.y = m->y.x; + mout->y.x = cp; + + cp = m->x.z; + mout->x.z = m->z.x; + mout->z.x = cp; + + cp = m->y.z; + mout->y.z = m->z.y; + mout->z.y = cp; + + return GO_RESULT_OK; +} + +int go_mat_cart_mult(const go_mat * m, const go_cart * v, + go_cart * vout) +{ + go_cart cp; + + cp = *v; + + vout->x = m->x.x * cp.x + m->y.x * cp.y + m->z.x * cp.z; + vout->y = m->x.y * cp.x + m->y.y * cp.y + m->z.y * cp.z; + vout->z = m->x.z * cp.x + m->y.z * cp.y + m->z.z * cp.z; + + return GO_RESULT_OK; +} + +int go_mat_mat_mult(const go_mat * m1, const go_mat * m2, go_mat * mout) +{ + go_mat cp1, cp2; + + cp1 = *m1; + cp2 = *m2; + + mout->x.x = cp1.x.x * cp2.x.x + cp1.y.x * cp2.x.y + cp1.z.x * cp2.x.z; + mout->x.y = cp1.x.y * cp2.x.x + cp1.y.y * cp2.x.y + cp1.z.y * cp2.x.z; + mout->x.z = cp1.x.z * cp2.x.x + cp1.y.z * cp2.x.y + cp1.z.z * cp2.x.z; + + mout->y.x = cp1.x.x * cp2.y.x + cp1.y.x * cp2.y.y + cp1.z.x * cp2.y.z; + mout->y.y = cp1.x.y * cp2.y.x + cp1.y.y * cp2.y.y + cp1.z.y * cp2.y.z; + mout->y.z = cp1.x.z * cp2.y.x + cp1.y.z * cp2.y.y + cp1.z.z * cp2.y.z; + + mout->z.x = cp1.x.x * cp2.z.x + cp1.y.x * cp2.z.y + cp1.z.x * cp2.z.z; + mout->z.y = cp1.x.y * cp2.z.x + cp1.y.y * cp2.z.y + cp1.z.y * cp2.z.z; + mout->z.z = cp1.x.z * cp2.z.x + cp1.y.z * cp2.z.y + cp1.z.z * cp2.z.z; + + return GO_RESULT_OK; +} + +/* go_quat functions */ + +go_flag go_quat_quat_compare(const go_quat * q1, const go_quat * q2) +{ + if (GO_ROT_CLOSE(q1->s, q2->s) && + GO_TRAN_CLOSE(q1->x, q2->x) && + GO_TRAN_CLOSE(q1->y, q2->y) && + GO_TRAN_CLOSE(q1->z, q2->z)) { + return 1; + } + + /* note (0, x, y, z) = (0, -x, -y, -z) */ + if (! GO_ROT_SMALL(q1->s) || + GO_TRAN_CLOSE(q1->x, -q2->x) || + GO_TRAN_CLOSE(q1->y, -q2->y) || + GO_TRAN_CLOSE(q1->z, -q2->z)) { + return 0; + } + + return 1; +} + +int go_quat_mag(const go_quat * quat, go_real * d) +{ + go_real sh; + + sh = sqrt(go_sq(quat->x) + go_sq(quat->y) + go_sq(quat->z)); + + *d = 2 * atan2(sh, quat->s); + + return GO_RESULT_OK; +} + +int go_quat_unit(const go_quat * q1, go_quat * qout) +{ + go_real d; + + (void) go_quat_mag(q1, &d); + if (GO_SMALL(d)) return GO_RESULT_ERROR; + d = 1.0 / d; + + return go_quat_scale_mult(q1, d, qout); +} + +int go_quat_norm(const go_quat * q1, go_quat * qout) +{ + go_real size; + + size = sqrt(go_sq(q1->s) + go_sq(q1->x) + go_sq(q1->y) + go_sq(q1->z)); + + if (GO_ROT_SMALL(size)) { + qout->s = 1; + qout->x = 0; + qout->y = 0; + qout->z = 0; + return GO_RESULT_NORM_ERROR; + } + size = 1.0 / size; + + if (q1->s >= 0) { + qout->s = q1->s * size; + qout->x = q1->x * size; + qout->y = q1->y * size; + qout->z = q1->z * size; + + return GO_RESULT_OK; + } else { + qout->s = -q1->s * size; + qout->x = -q1->x * size; + qout->y = -q1->y * size; + qout->z = -q1->z * size; + + return GO_RESULT_OK; + } +} + +int go_quat_inv(const go_quat * q1, go_quat * qout) +{ + qout->s = q1->s; + qout->x = -q1->x; + qout->y = -q1->y; + qout->z = -q1->z; + + if (!go_quat_is_norm(q1)) { + return GO_RESULT_NORM_ERROR; + } + + return GO_RESULT_OK; +} + +go_flag go_quat_is_norm(const go_quat * q1) +{ + return GO_TRAN_CLOSE(go_sq(q1->s) + + go_sq(q1->x) + + go_sq(q1->y) + + go_sq(q1->z), 1); +} + +int go_quat_scale_mult(const go_quat * q, go_real s, go_quat * qout) +{ + go_real sh; /* sine of half angle */ + go_real ha; /* half angle */ + go_real scale; /* new sh / old sh */ + + sh = sqrt(go_sq(q->x) + go_sq(q->y) + go_sq(q->z)); + if (GO_SMALL(sh)) { + /* zero rotation-- leave it alone */ + *qout = *q; + return GO_RESULT_OK; + } + + ha = atan2(sh, q->s); + ha *= s; + scale = sin(ha) / sh; + + qout->s = cos(ha); + qout->x = scale * q->x; + qout->y = scale * q->y; + qout->z = scale * q->z; + + return GO_RESULT_OK; +} + +int go_quat_quat_mult(const go_quat * q1, const go_quat * q2, + go_quat * qout) +{ + go_quat cp1, cp2; + + if (!go_quat_is_norm(q1) || !go_quat_is_norm(q2)) { + return GO_RESULT_NORM_ERROR; + } + + cp1 = *q1; + cp2 = *q2; + + qout->s = cp1.s * cp2.s - cp1.x * cp2.x - cp1.y * cp2.y - cp1.z * cp2.z; + + if (qout->s >= 0) { + qout->x = cp1.s * cp2.x + cp1.x * cp2.s + cp1.y * cp2.z - cp1.z * cp2.y; + qout->y = cp1.s * cp2.y - cp1.x * cp2.z + cp1.y * cp2.s + cp1.z * cp2.x; + qout->z = cp1.s * cp2.z + cp1.x * cp2.y - cp1.y * cp2.x + cp1.z * cp2.s; + } else { + qout->s = -qout->s; + qout->x = -cp1.s * cp2.x - cp1.x * cp2.s - cp1.y * cp2.z + cp1.z * cp2.y; + qout->y = -cp1.s * cp2.y + cp1.x * cp2.z - cp1.y * cp2.s - cp1.z * cp2.x; + qout->z = -cp1.s * cp2.z - cp1.x * cp2.y + cp1.y * cp2.x - cp1.z * cp2.s; + } + + return GO_RESULT_OK; +} + +int go_quat_cart_mult(const go_quat * q1, const go_cart * v2, + go_cart * vout) +{ + go_cart c; + + if (!go_quat_is_norm(q1)) { + return GO_RESULT_NORM_ERROR; + } + + c.x = q1->y * v2->z - q1->z * v2->y; + c.y = q1->z * v2->x - q1->x * v2->z; + c.z = q1->x * v2->y - q1->y * v2->x; + + vout->x = v2->x + 2 * (q1->s * c.x + q1->y * c.z - q1->z * c.y); + vout->y = v2->y + 2 * (q1->s * c.y + q1->z * c.x - q1->x * c.z); + vout->z = v2->z + 2 * (q1->s * c.z + q1->x * c.y - q1->y * c.x); + + return GO_RESULT_OK; +} + +/* go_pose functions*/ + +go_flag go_pose_pose_compare(const go_pose * p1, const go_pose * p2) +{ + return (go_quat_quat_compare(&p1->rot, &p2->rot) && + go_cart_cart_compare(&p1->tran, &p2->tran)); +} + +int go_pose_inv(const go_pose * p1, go_pose * p2) +{ + int retval; + + retval = go_quat_inv(&p1->rot, &p2->rot); + if (GO_RESULT_OK != retval) return retval; + + retval = go_quat_cart_mult(&p2->rot, &p1->tran, &p2->tran); + if (GO_RESULT_OK != retval) return retval; + + p2->tran.x = -p2->tran.x; + p2->tran.y = -p2->tran.y; + p2->tran.z = -p2->tran.z; + + return GO_RESULT_OK; +} + +int go_pose_cart_mult(const go_pose * p1, const go_cart * v2, go_cart * vout) +{ + int retval; + + /* first rotate the vector */ + retval = go_quat_cart_mult(&p1->rot, v2, vout); + if (GO_RESULT_OK != retval) return retval; + + /* then translate it */ + return go_cart_cart_add(&p1->tran, vout, vout); +} + +int go_pose_pose_mult(const go_pose * p1, const go_pose * p2, go_pose * pout) +{ + go_pose out; + int retval; + + retval = go_quat_cart_mult(&p1->rot, &p2->tran, &out.tran); + if (GO_RESULT_OK != retval) return retval; + + retval = go_cart_cart_add(&p1->tran, &out.tran, &out.tran); + if (GO_RESULT_OK != retval) return retval; + + retval = go_quat_quat_mult(&p1->rot, &p2->rot, &out.rot); + + *pout = out; + + return retval; +} + +int go_pose_scale_mult(const go_pose * p1, go_real s, go_pose * pout) +{ + (void) go_cart_scale_mult(&p1->tran, s, &pout->tran); + return go_quat_scale_mult(&p1->rot, s, &pout->rot); +} + +int go_pose_pose_interp(go_real t1, const go_pose * p1, go_real t2, const go_pose * p2, go_real t3, go_pose * p3) +{ + go_pose pdiff; + int retval; + + if (GO_CLOSE(t1, t2)) return GO_RESULT_ERROR; + + retval = go_pose_inv(p1, &pdiff); + if (GO_RESULT_OK != retval) return retval; + + (void) go_pose_pose_mult(&pdiff, p2, &pdiff); + (void) go_pose_scale_mult(&pdiff, (t3 - t1) / (t2 - t1), &pdiff); + return go_pose_pose_mult(p1, &pdiff, p3); +} + +/* homogeneous transform functions */ + +int go_hom_inv(const go_hom * h1, go_hom * h2) +{ + int retval; + + retval = go_mat_inv(&h1->rot, &h2->rot); + if (GO_RESULT_OK != retval) return retval; + + retval = go_mat_cart_mult(&h2->rot, &h1->tran, &h2->tran); + if (GO_RESULT_OK != retval) return retval; + + h2->tran.x = -h2->tran.x; + h2->tran.y = -h2->tran.y; + h2->tran.z = -h2->tran.z; + + return GO_RESULT_OK; +} + +int go_pose_screw_mult(const go_pose * pose, const go_screw * screw, go_screw * out) +{ + go_pose poseinv; + go_cart wxp; + go_cart v; + int retval; + + /* + B |B . B | + .T = | R . P | + A |A . A| + + B B A A A + .v = R ( v + w X P ) + . A B + + B B A + .w = R w + . A + */ + + retval = go_pose_inv(pose, &poseinv); + if (GO_RESULT_OK != retval) return retval; + + go_cart_cart_cross(&screw->w, &poseinv.tran, &wxp); + go_cart_cart_add(&screw->v, &wxp, &v); + go_quat_cart_mult(&pose->rot, &v, &out->v); + go_quat_cart_mult(&pose->rot, &screw->w, &out->w); + + return GO_RESULT_OK; +} + +/* line and plane functions */ + +int go_line_from_poGO_RESULT_direction(const go_cart * point, const go_cart * direction, go_line * line) +{ + int retval; + + retval = go_cart_unit(direction, &line->direction); + if (GO_RESULT_OK != retval) return GO_RESULT_DIV_ERROR; + + line->point = *point; + + return GO_RESULT_OK; +} + +int go_line_from_points(const go_cart * point1, const go_cart * point2, go_line * line) +{ + go_cart direction; + int retval; + + (void) go_cart_cart_sub(point2, point1, &direction); + retval = go_cart_unit(&direction, &line->direction); + if (GO_RESULT_OK != retval) return GO_RESULT_DIV_ERROR; + + line->point = *point1; + + return GO_RESULT_OK; +} + +int go_line_from_planes(const go_plane * plane1, const go_plane * plane2, go_line * line) +{ + enum {X, Y, Z} which; + go_real max, ymax, denominv; + + (void) go_cart_cart_cross(&plane1->normal, &plane2->normal, &line->direction); + if (GO_RESULT_OK != go_cart_unit(&line->direction, &line->direction)) { + /* planes are parallel */ + return GO_RESULT_ERROR; + } + + /* pick the cross product component with the largest magnitude, + which will give the most robust calculations for the point + on the line */ + max = fabs(line->direction.x); + which = X; + ymax = fabs(line->direction.y); + if (ymax > max) { + max = ymax; + which = Y; + } + if (fabs(line->direction.z) > max) { + which = Z; + } + + switch (which) { + case X: + denominv = 1.0/(plane1->normal.y*plane2->normal.z - + plane2->normal.y*plane1->normal.z); + line->point.y = (plane1->normal.z*plane2->d - + plane2->normal.z*plane1->d)*denominv; + line->point.z = (plane2->normal.y*plane1->d - + plane1->normal.y*plane2->d)*denominv; + line->point.x = 0; + break; + case Y: + denominv = 1.0/(plane1->normal.z*plane2->normal.x - + plane2->normal.z*plane1->normal.x); + line->point.z = (plane1->normal.x*plane2->d - + plane2->normal.x*plane1->d)*denominv; + line->point.x = (plane2->normal.z*plane1->d - + plane1->normal.z*plane2->d)*denominv; + line->point.y = 0; + break; + default: + denominv = 1.0/(plane1->normal.x*plane2->normal.y - + plane2->normal.x*plane1->normal.y); + line->point.x = (plane1->normal.y*plane2->d - + plane2->normal.y*plane1->d)*denominv; + line->point.y = (plane2->normal.x*plane1->d - + plane1->normal.x*plane2->d)*denominv; + line->point.z = 0; + break; + } + + return GO_RESULT_OK; +} + +go_flag go_line_line_compare(const go_line * line1, const go_line * line2) +{ + go_cart diff; + go_real dot; + + /* check that directions are parallel */ + go_cart_cart_dot(&line1->direction, &line2->direction, &dot); + if (! GO_CLOSE(dot, 1)) return 0; + + /* check that points are along the direction */ + (void) go_cart_cart_sub(&line1->point, &line2->point, &diff); + go_cart_cart_dot(&line1->direction, &diff, &dot); + if (! GO_CLOSE(dot, 1)) return 0; + + return 1; +} + +int go_line_evaluate(const go_line * line, go_real d, go_cart * point) +{ + go_cart v; + + (void) go_cart_scale_mult(&line->direction, d, &v); + (void) go_cart_cart_add(&line->point, &v, point); + + return GO_RESULT_OK; +} + +int go_poGO_RESULT_line_distance(const go_cart * point, const go_line * line, go_real * distance) +{ + *distance = sqrt( + go_sq(line->direction.z * (point->y - line->point.y) - + line->direction.y * (point->z - line->point.z)) + + go_sq(line->direction.x * (point->z - line->point.z) - + line->direction.z * (point->x - line->point.x)) + + go_sq(line->direction.y * (point->x - line->point.x) - + line->direction.x * (point->y - line->point.y))); + + return GO_RESULT_OK; +} + +int go_poGO_RESULT_line_proj(const go_cart * point, const go_line * line, go_cart * pout) +{ + go_cart vp; + int retval; + + go_cart_cart_sub(point, &line->point, &vp); + retval = go_cart_cart_proj(&vp, &line->direction, &vp); + if (GO_RESULT_OK != retval) return retval; + go_cart_cart_add(&line->point, &vp, pout); + + return GO_RESULT_OK; +} + +int go_poGO_RESULT_plane_proj(const go_cart * point, const go_plane * plane, go_cart * proj) +{ + go_real denom; + go_real k; + + denom = + go_sq(plane->normal.x) + + go_sq(plane->normal.y) + + go_sq(plane->normal.z); + + if (GO_TRAN_SMALL(denom)) return GO_RESULT_DIV_ERROR; + + k = -(plane->normal.x * point->x + + plane->normal.y * point->y + + plane->normal.z * point->z + + plane->d)/denom; + + proj->x = point->x + k*plane->normal.x; + proj->y = point->y + k*plane->normal.y; + proj->z = point->z + k*plane->normal.z; + + return GO_RESULT_OK; +} + +int go_line_plane_proj(const go_line * line, const go_plane * plane, go_line * proj) +{ + int retval; + + retval = go_cart_plane_proj(&line->direction, &plane->normal, &proj->direction); + if (GO_RESULT_OK != retval) return retval; + + return go_poGO_RESULT_plane_proj(&line->point, plane, &proj->point); +} + +int go_plane_from_poGO_RESULT_normal(const go_cart * point, const go_cart * normal, go_plane * plane) +{ + if (GO_RESULT_OK != go_cart_unit(normal, &plane->normal)) return GO_RESULT_ERROR; + /* D = -(Ax + By + Cz) */ + plane->d = -(plane->normal.x * point->x + + plane->normal.y * point->y + + plane->normal.z * point->z); + + return GO_RESULT_OK; +} + +int go_plane_from_abcd(go_real A, go_real B, go_real C, go_real D, go_plane * plane) +{ + go_real mag; + + /* get magnitude of normal vector, actually mag squared */ + mag = go_sq(A) + go_sq(B) + go_sq(C); + if (GO_TRAN_SMALL(mag)) { + return GO_RESULT_DIV_ERROR; + } + mag = 1.0 / sqrt(mag); /* now it's the mag multiplier */ + + plane->normal.x = A * mag; + plane->normal.y = B * mag; + plane->normal.z = C * mag; + plane->d = D * mag; + + return GO_RESULT_OK; +} + +int go_plane_from_points(const go_cart * point1, const go_cart * point2, const go_cart * point3, go_plane * plane) +{ + go_cart v12, v23; + + /* cross vectors from 1-2, 2-3 to get normal */ + go_cart_cart_sub(point2, point1, &v12); + go_cart_cart_sub(point3, point2, &v23); + go_cart_cart_cross(&v12, &v23, &plane->normal); + if (GO_RESULT_OK != go_cart_unit(&plane->normal, &plane->normal)) return GO_RESULT_ERROR; + /* D = -(Ax + By + Cz) */ + plane->d = -(plane->normal.x * point1->x + + plane->normal.y * point1->y + + plane->normal.z * point1->z); + + return GO_RESULT_OK; +} + +int go_plane_from_poGO_RESULT_line(const go_cart * point, const go_line * line, go_plane * plane) +{ + go_cart p0, p1; + + (void) go_line_evaluate(line, 0, &p0); + (void) go_line_evaluate(line, 1, &p1); + + return go_plane_from_points(&p0, &p1, point, plane); +} + +/* for planes to be the same, they must have the same normal vector + and same 'd', so the comparison is simple */ +go_flag go_plane_plane_compare(const go_plane * plane1, const go_plane * plane2) +{ + if (! go_cart_cart_compare(&plane1->normal, &plane2->normal)) return 0; + return GO_CLOSE(plane1->d, plane2->d); +} + +int go_poGO_RESULT_plane_distance(const go_cart * point, const go_plane * plane, go_real * distance) +{ + *distance = plane->normal.x * point->x + + plane->normal.y * point->y + + plane->normal.z * point->z + + plane->d; + + return GO_RESULT_OK; +} + +int go_plane_evaluate(const go_plane * plane, go_real u, go_real v, go_cart * point) +{ + go_cart v1, v2; /* orthogonal vectors in plane */ + go_cart p; /* point in plane closest to origin */ + + if (GO_RESULT_OK != go_cart_normal(&plane->normal, &v1)) return GO_RESULT_ERROR; + + (void) go_cart_cart_cross(&plane->normal, &v1, &v2); + (void) go_cart_scale_mult(&v1, u, &v1); + (void) go_cart_scale_mult(&v2, v, &v2); + + /* with point = k * normal, + a*ka * y*ky + z*kz + d = 0, + k*(1) + d = 0, + k = -d + */ + p.x = -plane->normal.x * plane->d; + p.y = -plane->normal.y * plane->d; + p.z = -plane->normal.z * plane->d; + + (void) go_cart_cart_add(&p, &v1, &p); + (void) go_cart_cart_add(&p, &v2, point); + + return GO_RESULT_OK; +} + +/* + To get the intersect point of the line and plane, set them equal and + solve for the line parameter d, then plug into the parameterization + to get the point: + + A(px + d*vx) + B(py + d*vy) + C(pz + d*vz) + D = 0 + + _ -(A*px + B*py + C*pz + D) + d --------------------- + _ A*vx + B*vy + C*vz + + Then plug d into px + d*vx, ... to get intersect x, y and z. +*/ +int go_line_plane_intersect(const go_line * line, const go_plane * plane, go_cart * point, go_real * distance) +{ + go_real num, denom; + + /* first check for line parallel to plane */ + (void) go_cart_cart_dot(&plane->normal, &line->direction, &denom); + if (GO_SMALL(denom)) return GO_RESULT_ERROR; + + /* check for point lying in plane */ + (void) go_cart_cart_dot(&plane->normal, &line->point, &num); + num += plane->d; + if (GO_SMALL(num)) { + *point = line->point; + /* distance probably isn't exactly zero, so compute it */ + return go_poGO_RESULT_plane_distance(&line->point, plane, distance); + } + + /* else plug in d to get intersect point */ + *distance = -num / denom; + return go_line_evaluate(line, *distance, point); +} + +/* general vector and matrix functions */ + +/* + Adapted from ludcmp routine in Numerical Recipes in C, but with + indices starting at 0 and no heap allocation. + + Given a matrix a[0..n-1][0..n-1], this routine replaces it by the LU + decomposition of a rowwise permutation of itself. a and n are + input. a is changed and output. indx[0..n-1] is an output vector + that records the row permutation effected by the partial pivoting; d + is output as +/-1 depending on whether the number of row + interchanges was even or odd, respectively. This routine is used in + combination with lubksb to solve linear equations or invert a + matrix. + + Warning! The matrix 'a' can't be declared as go_real a[n][n]. It + needs to be an array of go_real pointers. See e.g. go_mat6_inv() for + how to set up the 'a' matrix. +*/ + +static go_real go_singular_epsilon = 1.0e-15; + +go_real go_get_singular_epsilon(void) +{ + return go_singular_epsilon; +} + +int go_set_singular_epsilon(go_real epsilon) +{ + if (epsilon <= 0.0) return GO_RESULT_ERROR; + + go_singular_epsilon = epsilon; + + return GO_RESULT_OK; +} + +int ludcmp(go_real ** a, + go_real * scratchrow, + go_integer n, + go_integer * indx, + go_real * d) +{ + go_integer i, imax, j, k; + go_real big, dum, sum, temp; + + *d = 1.0; + for (i = 0; i < n; i++) { + big = 0.0; + for (j = 0; j < n; j++) + if ((temp = fabs(a[i][j])) > big) + big = temp; + if (big < go_singular_epsilon) + return GO_RESULT_SINGULAR; + scratchrow[i] = 1.0 / big; + } + for (j = 0; j < n; j++) { + for (i = 0; i < j; i++) { + sum = a[i][j]; + for (k = 0; k < i; k++) + sum -= a[i][k] * a[k][j]; + a[i][j] = sum; + } + big = 0.0; + imax = 0; + for (i = j; i < n; i++) { + sum = a[i][j]; + for (k = 0; k < j; k++) + sum -= a[i][k] * a[k][j]; + a[i][j] = sum; + if ((dum = scratchrow[i] * fabs(sum)) >= big) { + big = dum; + imax = i; + } + } + if (j != imax) { + for (k = 0; k < n; k++) { + dum = a[imax][k]; + a[imax][k] = a[j][k]; + a[j][k] = dum; + } + *d = -(*d); + scratchrow[imax] = scratchrow[j]; + } + indx[j] = imax; + if (fabs(a[j][j]) < go_singular_epsilon) + return GO_RESULT_SINGULAR; + if (j != n - 1) { + dum = 1.0 / (a[j][j]); + for (i = j + 1; i < n; i++) + a[i][j] *= dum; + } + } + + return GO_RESULT_OK; +} + +/* + Solves the set of n linear equations A·X = B. Here a[0..n-1][0..n-1] + is input, not as the matrix A but rather as its LU decomposition, + determined by the routine ludcmp. indx[0..n-1] is input as the + permutation vector returned by ludcmp. b[0..n-1] is input as the + right-hand side vector B, and returns with the solution vector X. a, + n, and indx are not modified by this routine and can be left in + place for successive calls with different right-hand sides b. This + routine takes into account the possibility that b will begin with + many zero elements, so it is efficient for use in matrix inversion. + + Warning! The matrix 'a' can't be declared as go_real a[n][n]. It + needs to be an array of go_real pointers. See e.g. go_mat6_inv() for + how to set up the 'a' matrix. +*/ + +int lubksb(go_real ** a, + go_integer n, + go_integer * indx, + go_real * b) +{ + go_integer i, ii = -1, ip, j; + go_real sum; + + for (i = 0; i < n; i++) { + ip = indx[i]; + sum = b[ip]; + b[ip] = b[i]; + if (ii != -1) + for (j = ii; j <= i - 1; j++) + sum -= a[i][j] * b[j]; + else if (sum) + ii = i; + b[i] = sum; + } + for (i = n - 1; i >= 0; i--) { + sum = b[i]; + for (j = i + 1; j < n; j++) + sum -= a[i][j] * b[j]; + if (fabs(a[i][i]) < go_singular_epsilon) + return GO_RESULT_SINGULAR; + b[i] = sum / a[i][i]; + } + + return GO_RESULT_OK; +} + +int go_cart_vector_convert(const go_cart * c, + go_real * v) +{ + v[0] = c->x, v[1] = c->y, v[2] = c->z; + + return GO_RESULT_OK; +} + +int go_vector_cart_convert(const go_real * v, + go_cart * c) +{ + c->x = v[0], c->y = v[1], c->z = v[2]; + + return GO_RESULT_OK; +} + +int go_quat_matrix_convert(const go_quat * quat, + go_matrix * matrix) +{ + go_mat mat; + int retval; + + /* check for an initialized matrix */ + if (0 == matrix->el[0]) return GO_RESULT_ERROR; + /* check for a 3x3 matrix */ + if (matrix->rows != 3 || matrix->cols != 3) return GO_RESULT_ERROR; + + retval = go_quat_mat_convert(quat, &mat); + if (GO_RESULT_OK != retval) return retval; + + return go_mat_matrix_convert(&mat, matrix); +} + +/* | m.x.x m.y.x m.z.x | */ +/* go_mat m = | m.x.y m.y.y m.z.y | */ +/* | m.x.z m.y.z m.z.z | */ +/* go_matrix mout[row][col] */ + +int go_mat_matrix_convert(const go_mat * mat, + go_matrix * matrix) +{ + /* check for an initialized matrix */ + if (0 == matrix->el[0]) return GO_RESULT_ERROR; + /* check for a 3x3 matrix */ + if (matrix->rows != 3 || matrix->cols != 3) return GO_RESULT_ERROR; + + matrix->el[0][0] = mat->x.x, matrix->el[0][1] = mat->y.x, matrix->el[0][2] = mat->z.x; + matrix->el[1][0] = mat->x.y, matrix->el[1][1] = mat->y.y, matrix->el[1][2] = mat->z.y; + matrix->el[2][0] = mat->x.z, matrix->el[2][1] = mat->y.z, matrix->el[2][2] = mat->z.z; + + return GO_RESULT_OK; +} + +int go_matrix_matrix_add(const go_matrix * a, + const go_matrix * b, + go_matrix * apb) +{ + go_integer row, col; + + /* check for an initialized matrix */ + if (0 == a->el[0] || 0 == b->el[0] || 0 == apb->el[0]) return GO_RESULT_ERROR; + /* check for matching rows and cols */ + if (a->rows != b->rows || a->cols != b->cols || + b->rows != apb->rows || b->cols != apb->cols) return GO_RESULT_ERROR; + + for (row = 0; row < a->rows; row++) { + for (col = 0; col < a->cols; col++) { + apb->el[row][col] = a->el[row][col] + b->el[row][col]; + } + } + + return GO_RESULT_OK; +} + +int go_matrix_matrix_copy(const go_matrix * src, + go_matrix * dst) +{ + go_integer row, col; + + /* check for an initialized matrix */ + if (0 == src->el[0] || 0 == dst->el[0]) return GO_RESULT_ERROR; + + /* check for matching rows and cols */ + if (src->rows != dst->rows || src->cols != dst->cols) return GO_RESULT_ERROR; + + for (row = 0; row < src->rows; row++) { + for (col = 0; col < src->cols; col++) { + dst->el[row][col] = src->el[row][col]; + } + } + + return GO_RESULT_OK; +} + +int go_matrix_matrix_mult(const go_matrix * a, + const go_matrix * b, + go_matrix * ab) +{ + go_real ** ptrin; + go_real ** ptrout; + go_integer row, col, i; + + /* check for an initialized matrix */ + if (0 == a->el[0] || 0 == b->el[0] || 0 == ab->el[0]) return GO_RESULT_ERROR; + /* check for consistent rows and cols */ + if (a->cols != b->rows || + a->rows != ab->rows || + b->cols != ab->cols) return GO_RESULT_ERROR; + + if (ab == a) { + /* destructive multiply, use a's copy space and copy back */ + ptrin = a->elcpy; + ptrout = a->el; + } else if (ab == b) { + ptrin = b->elcpy; + ptrout = b->el; + } else { + ptrin = ab->el; + ptrout = NULL; + } + + for (row = 0; row < a->rows; row++) { + for (col = 0; col < b->cols; col++) { + ptrin[row][col] = 0; + for (i = 0; i < a->cols; i++) { + ptrin[row][col] += a->el[row][i] * b->el[i][col]; + } + } + } + + if (NULL != ptrout) { + for (row = 0; row < ab->rows; row++) { + for (col = 0; col < ab->cols; col++) { + ptrout[row][col] = ptrin[row][col]; + } + } + } + + return GO_RESULT_OK; +} + +int go_matrix_vector_mult(const go_matrix * a, + const go_vector * v, + go_vector * axv) +{ + go_vector * ptrin; + go_vector * ptrout; + go_integer row, i; + + /* check for an initialized matrix */ + if (0 == a->el[0]) return GO_RESULT_ERROR; + + if (axv == v) { + ptrin = a->elcpy[0]; + ptrout = axv; + } else { + ptrin = axv; + ptrout = NULL; + } + + for (row = 0; row < a->rows; row++) { + ptrin[row] = 0; + for (i = 0; i < a->cols; i++) { + ptrin[row] += a->el[row][i] * v[i]; + } + } + + if (ptrout != NULL) { + for (row = 0; row < a->rows; row++) { + ptrout[row] = ptrin[row]; + } + } + + return GO_RESULT_OK; +} + +/* + The matrix-vector cross product is a matrix of the same dimension, + whose columns are the column-wise cross products of the matrix + and the vector. The matrices must be 3xN, the vector 3x1. +*/ +int go_matrix_vector_cross(const go_matrix * a, + const go_vector * v, + go_matrix * axv) +{ + go_real ** ptrin; + go_real ** ptrout; + go_cart vc; /* 'v' */ + go_cart ac; /* a column of the 'a' matrix */ + go_cart cross; + go_integer row, col; + + /* check for an initialized matrix */ + if (0 == a->el[0] || 0 == axv->el[0]) return GO_RESULT_ERROR; + /* check for consistent rows and cols */ + if (a->rows != 3 || + axv->rows != 3 || + a->cols != axv->cols) return GO_RESULT_ERROR; + + if (axv == a) { + /* destructive multiply, use a's copy space and copy back */ + ptrin = a->elcpy; + ptrout = a->el; + } else { + ptrin = axv->el; + ptrout = NULL; + } + + /* get 'v' as a cartesian type */ + vc.x = v[0], vc.y = v[1], vc.z = v[2]; + + for (col = 0; col < a->cols; col++) { + /* pick off the col'th column as a cartesian type */ + ac.x = a->el[0][col], ac.y = a->el[1][col], ac.z = a->el[2][col]; + /* cross it with v */ + go_cart_cart_cross(&ac, &vc, &cross); + /* make it the col'th column of axv[] */ + ptrin[0][col] = cross.x, ptrin[1][col] = cross.y, ptrin[2][col] = cross.z; + } + + if (ptrout != NULL) { + for (row = 0; row < a->rows; row++) { + for (col = 0; col < a->cols; col++) { + ptrout[row][col] = ptrin[row][col]; + } + } + } + + return GO_RESULT_OK; +} + +int go_matrix_transpose(const go_matrix * a, + go_matrix * at) +{ + go_real ** ptrin; + go_real ** ptrout; + go_integer row, col; + + /* check for fixed matrix */ + if (0 == a->el[0] || 0 == at->el[0]) return GO_RESULT_ERROR; + + if (at == a) { + ptrin = a->elcpy; + ptrout = a->el; + } else { + ptrin = at->el; + ptrout = NULL; + } + + for (row = 0; row < a->rows; row++) { + for (col = 0; col < a->cols; col++) { + ptrin[col][row] = a->el[row][col]; + } + } + + if (ptrout != NULL) { + for (row = 0; row < a->rows; row++) { + for (col = 0; col < a->cols; col++) { + ptrout[row][col] = ptrin[row][col]; + } + } + } + + return GO_RESULT_OK; +} + +int go_matrix_inv(const go_matrix * m, /* M x N */ + go_matrix * minv) /* N x M */ +{ + go_real d; + go_integer N, row, col; + int retval; + + /* check for fixed matrix */ + if (0 == m->el[0] || 0 == minv->el[0]) return GO_RESULT_ERROR; + + N = m->rows; + + /* copy of m since ludcmp destroys input matrix */ + for (row = 0; row < N; row++) { + for (col = 0; col < N; col++) { + m->elcpy[row][col] = m->el[row][col]; + } + } + + /* convert the copy to its LU decomposition */ + retval = ludcmp(m->elcpy, m->v, N, m->index, &d); + if (GO_RESULT_OK != retval) return retval; + + /* backsubstitute a column with a 1 in it to get the inverse */ + for (col = 0; col < N; col++) { + for (row = 0; row < N; row++) { + m->v[row] = 0.0; + } + m->v[col] = 1.0; + retval = lubksb(m->elcpy, N, m->index, m->v); + if (GO_RESULT_OK != retval) return retval; + for (row = 0; row < N; row++) { + minv->el[row][col] = m->v[row]; + } + } + + return GO_RESULT_OK; +} + +extern int go_mat3_inv(const go_real a[3][3], + go_real ainv[3][3]) +{ + go_real a11a22; + go_real a11a23; + go_real a11a32; + go_real a11a33; + + go_real a12a33; + go_real a12a23; + go_real a12a21; + go_real a12a31; + + go_real a13a21; + go_real a13a22; + go_real a13a31; + go_real a13a32; + + go_real a21a32; + go_real a21a33; + + go_real a22a31; + go_real a22a33; + + go_real a23a31; + go_real a23a32; + + go_real denom; + + a11a22 = a[0][0]*a[1][1]; + a11a23 = a[0][0]*a[1][2]; + a11a32 = a[0][0]*a[2][1]; + a11a33 = a[0][0]*a[2][2]; + + a12a21 = a[0][1]*a[1][0]; + a12a23 = a[0][1]*a[1][2]; + a12a31 = a[0][1]*a[2][0]; + a12a33 = a[0][1]*a[2][2]; + + a13a21 = a[0][2]*a[1][0]; + a13a22 = a[0][2]*a[1][1]; + a13a31 = a[0][2]*a[2][0]; + a13a32 = a[0][2]*a[2][1]; + + a21a32 = a[1][0]*a[2][1]; + a21a33 = a[1][0]*a[2][2]; + + a22a31 = a[1][1]*a[2][0]; + a22a33 = a[1][1]*a[2][2]; + + a23a31 = a[1][2]*a[2][0]; + a23a32 = a[1][2]*a[2][1]; + + denom = + + a11a22*a[2][2] + - a11a23*a[2][1] + - a12a33*a[1][0] + + a13a21*a[2][1] + - a13a22*a[2][0] + + a12a23*a[2][0]; + + if (GO_SMALL(denom)) { + return GO_RESULT_SINGULAR; + } + + denom = 1.0/denom; + + ainv[0][0] = (a22a33 - a23a32)*denom; + ainv[0][1] = (a13a32 - a12a33)*denom; + ainv[0][2] = (a12a23 - a13a22)*denom; + + ainv[1][0] = (a23a31 - a21a33)*denom; + ainv[1][1] = (a11a33 - a13a31)*denom; + ainv[1][2] = (a13a21 - a11a23)*denom; + + ainv[2][0] = (a21a32 - a22a31)*denom; + ainv[2][1] = (a12a31 - a11a32)*denom; + ainv[2][2] = (a11a22 - a12a21)*denom; + + return GO_RESULT_OK; +} + +int go_mat3_mat3_mult(const go_real a[3][3], + const go_real b[3][3], + go_real axb[3][3]) +{ + go_real work[3][3]; + go_integer i, j, k; + + for (i = 0; i < 3; i++) { + for (j = 0; j < 3; j++) { + work[i][j] = 0; + for (k = 0; k < 3; k++) { + work[i][j] += a[i][k] * b[k][j]; + } + } + } + + for (i = 0; i < 3; i++) + for (j = 0; j < 3; j++) + axb[i][j] = work[i][j]; + + return GO_RESULT_OK; +} + +int go_mat3_vec3_mult(const go_real a[3][3], + const go_real v[3], + go_real axv[3]) +{ + go_real work[3]; + go_integer i, j; + + for (i = 0; i < 3; i++) { + work[i] = 0; + for (j = 0; j < 3; j++) { + work[i] += a[i][j] * v[j]; + } + } + + for (i = 0; i < 3; i++) + axv[i] = work[i]; + + return GO_RESULT_OK; +} + +extern int go_mat4_inv(const go_real a[4][4], + go_real ainv[4][4]) +{ + go_real work[4][4]; + go_real denom; + + go_real a11a22; + go_real a11a23; + go_real a11a24; + go_real a11a33; + go_real a11a32; + go_real a11a34; + + go_real a12a21; + go_real a12a23; + go_real a12a24; + go_real a12a31; + go_real a12a33; + go_real a12a34; + + go_real a13a21; + go_real a13a22; + go_real a13a24; + go_real a13a31; + go_real a13a32; + go_real a13a34; + + go_real a14a21; + go_real a14a22; + go_real a14a23; + go_real a14a31; + go_real a14a32; + go_real a14a33; + + go_real a21a32; + go_real a21a33; + go_real a21a34; + + go_real a22a31; + go_real a22a33; + go_real a22a34; + + go_real a23a31; + go_real a23a32; + go_real a23a34; + + go_real a24a31; + go_real a24a32; + go_real a24a33; + + go_real a11a22a33; + go_real a11a22a34; + go_real a11a23a32; + go_real a11a23a34; + go_real a11a24a32; + go_real a11a24a33; + go_real a12a21a33; + go_real a12a21a34; + go_real a12a23a31; + go_real a12a23a34; + go_real a12a24a31; + go_real a12a24a33; + go_real a13a21a32; + go_real a13a21a34; + go_real a13a22a31; + go_real a13a22a34; + go_real a13a24a31; + go_real a13a24a32; + go_real a14a21a32; + go_real a14a21a33; + go_real a14a22a31; + go_real a14a22a33; + go_real a14a23a31; + go_real a14a23a32; + + go_integer row, col; + + a11a22 = a[0][0]*a[1][1]; + a11a23 = a[0][0]*a[1][2]; + a11a24 = a[0][0]*a[1][3]; + a11a33 = a[0][0]*a[2][2]; + a11a32 = a[0][0]*a[2][1]; + a11a34 = a[0][0]*a[2][3]; + + a12a21 = a[0][1]*a[1][0]; + a12a23 = a[0][1]*a[1][2]; + a12a24 = a[0][1]*a[1][3]; + a12a31 = a[0][1]*a[2][0]; + a12a33 = a[0][1]*a[2][2]; + a12a34 = a[0][1]*a[2][3]; + + a13a21 = a[0][2]*a[1][0]; + a13a22 = a[0][2]*a[1][1]; + a13a24 = a[0][2]*a[1][3]; + a13a31 = a[0][2]*a[2][0]; + a13a32 = a[0][2]*a[2][1]; + a13a34 = a[0][2]*a[2][3]; + + a14a21 = a[0][3]*a[1][0]; + a14a22 = a[0][3]*a[1][1]; + a14a23 = a[0][3]*a[1][2]; + a14a31 = a[0][3]*a[2][0]; + a14a32 = a[0][3]*a[2][1]; + a14a33 = a[0][3]*a[2][2]; + + a21a32 = a[1][0]*a[2][1]; + a21a33 = a[1][0]*a[2][2]; + a21a34 = a[1][0]*a[2][3]; + + a22a31 = a[1][1]*a[2][0]; + a22a33 = a[1][1]*a[2][2]; + a22a34 = a[1][1]*a[2][3]; + + a23a31 = a[1][2]*a[2][0]; + a23a32 = a[1][2]*a[2][1]; + a23a34 = a[1][2]*a[2][3]; + + a24a31 = a[1][3]*a[2][0]; + a24a32 = a[1][3]*a[2][1]; + a24a33 = a[1][3]*a[2][2]; + + a11a22a33 = a11a22*a[2][2]; + a11a22a34 = a11a22*a[2][3]; + a11a23a32 = a11a23*a[2][1]; + a11a23a34 = a11a23*a[2][3]; + a11a24a32 = a11a24*a[2][1]; + a11a24a33 = a11a24*a[2][2]; + a12a21a33 = a12a21*a[2][2]; + a12a21a34 = a12a21*a[2][3]; + a12a23a31 = a12a23*a[2][0]; + a12a23a34 = a12a23*a[2][3]; + a12a24a31 = a12a24*a[2][0]; + a12a24a33 = a12a24*a[2][2]; + a13a21a32 = a13a21*a[2][1]; + a13a21a34 = a13a21*a[2][3]; + a13a22a31 = a13a22*a[2][0]; + a13a22a34 = a13a22*a[2][3]; + a13a24a31 = a13a24*a[2][0]; + a13a24a32 = a13a24*a[2][1]; + a14a21a32 = a14a21*a[2][1]; + a14a21a33 = a14a21*a[2][2]; + a14a22a31 = a14a22*a[2][0]; + a14a22a33 = a14a22*a[2][2]; + a14a23a31 = a14a23*a[2][0]; + a14a23a32 = a14a23*a[2][1]; + + denom = + + a12a24a33*a[3][0] + + a13a24a31*a[3][1] + - a12a21a33*a[3][3] + - a13a24a32*a[3][0] + - a14a21a32*a[3][2] + - a12a24a31*a[3][2] + + a14a23a32*a[3][0] + + a13a21a32*a[3][3] + + a14a21a33*a[3][1] + + a12a23a31*a[3][3] + - a14a23a31*a[3][1] + + a12a21a34*a[3][2] + - a12a23a34*a[3][0] + - a13a21a34*a[3][1] + - a13a22a31*a[3][3] + + a14a22a31*a[3][2] + + a13a22a34*a[3][0] + - a14a22a33*a[3][0] + + a11a22a33*a[3][3] + - a11a22a34*a[3][2] + + a11a24a32*a[3][2] + - a11a23a32*a[3][3] + + a11a23a34*a[3][1] + - a11a24a33*a[3][1]; + + if (GO_SMALL(denom)) { + return GO_RESULT_SINGULAR; + } + + denom = 1.0/denom; + + work[0][0] = (+ a22a33*a[3][3] + - a22a34*a[3][2] + + a24a32*a[3][2] + - a23a32*a[3][3] + + a23a34*a[3][1] + - a24a33*a[3][1])*denom; + work[0][1] = (- a12a33*a[3][3] + + a12a34*a[3][2] + - a13a34*a[3][1] + + a13a32*a[3][3] + - a14a32*a[3][2] + + a14a33*a[3][1])*denom; + work[0][2] = (+ a12a23*a[3][3] + - a12a24*a[3][2] + - a13a22*a[3][3] + + a13a24*a[3][1] + + a14a22*a[3][2] + - a14a23*a[3][1])*denom; + work[0][3] = (- a12a23a34 + + a12a24a33 + + a13a22a34 + - a13a24a32 + - a14a22a33 + + a14a23a32)*denom; + + work[1][0] = (+ a23a31*a[3][3] + - a23a34*a[3][0] + - a21a33*a[3][3] + + a21a34*a[3][2] + + a24a33*a[3][0] + - a24a31*a[3][2])*denom; + work[1][1] = (+ a11a33*a[3][3] + - a11a34*a[3][2] + - a13a31*a[3][3] + + a14a31*a[3][2] + + a13a34*a[3][0] + - a14a33*a[3][0])*denom; + work[1][2] = (- a11a23*a[3][3] + + a11a24*a[3][2] + + a13a21*a[3][3] + + a14a23*a[3][0] + - a14a21*a[3][2] + - a13a24*a[3][0])*denom; + work[1][3] = (+ a11a23a34 + - a11a24a33 + - a13a21a34 + + a14a21a33 + - a14a23a31 + + a13a24a31)*denom; + + work[2][0] = (+ a22a34*a[3][0] + - a22a31*a[3][3] + - a24a32*a[3][0] + + a21a32*a[3][3] + - a21a34*a[3][1] + + a24a31*a[3][1])*denom; + work[2][1] = (+ a11a34*a[3][1] + - a11a32*a[3][3] + - a12a34*a[3][0] + - a14a31*a[3][1] + + a12a31*a[3][3] + + a14a32*a[3][0])*denom; + work[2][2] = (- a11a24*a[3][1] + - a14a22*a[3][0] + + a11a22*a[3][3] + + a12a24*a[3][0] + + a14a21*a[3][1] + - a12a21*a[3][3])*denom; + work[2][3] = (- a11a22a34 + + a11a24a32 + + a14a22a31 + - a12a24a31 + + a12a21a34 + - a14a21a32)*denom; + + work[3][0] = (+ a21a33*a[3][1] + + a23a32*a[3][0] + - a22a33*a[3][0] + - a23a31*a[3][1] + + a22a31*a[3][2] + - a21a32*a[3][2])*denom; + work[3][1] = (+ a12a33*a[3][0] + - a13a32*a[3][0] + - a12a31*a[3][2] + + a13a31*a[3][1] + + a11a32*a[3][2] + - a11a33*a[3][1])*denom; + work[3][2] = (- a12a23*a[3][0] + + a12a21*a[3][2] + - a13a21*a[3][1] + + a13a22*a[3][0] + - a11a22*a[3][2] + + a11a23*a[3][1])*denom; + work[3][3] = (+ a13a21a32 + + a12a23a31 + - a12a21a33 + - a13a22a31 + + a11a22a33 + - a11a23a32)*denom; + + for (row = 0; row < 4; row++) + for (col = 0; col < 4; col++) + ainv[row][col] = work[row][col]; + + return GO_RESULT_OK; +} + +int go_mat4_mat4_mult(const go_real a[4][4], + const go_real b[4][4], + go_real axb[4][4]) +{ + go_real work[4][4]; + go_integer i, j, k; + + for (i = 0; i < 4; i++) { + for (j = 0; j < 4; j++) { + work[i][j] = 0; + for (k = 0; k < 4; k++) { + work[i][j] += a[i][k] * b[k][j]; + } + } + } + + for (i = 0; i < 4; i++) + for (j = 0; j < 4; j++) + axb[i][j] = work[i][j]; + + return GO_RESULT_OK; +} + +int go_mat4_vec4_mult(const go_real a[4][4], + const go_real v[4], + go_real axv[4]) +{ + go_real work[4]; + go_integer i, j; + + for (i = 0; i < 4; i++) { + work[i] = 0; + for (j = 0; j < 4; j++) { + work[i] += a[i][j] * v[j]; + } + } + + for (i = 0; i < 4; i++) + axv[i] = work[i]; + + return GO_RESULT_OK; +} + +int go_mat6_inv(const go_real a[6][6], + go_real ainv[6][6]) +{ + go_real cpy[6][6]; + go_real *cpyptr[6]; + go_real scratchrow[6]; + go_real v[6]; + go_real d; + go_integer index[6]; + go_integer row, col; + int retval; + + /* create a copy of m[][] in cpy[][], since ludcmp destroys input matrix */ + for (row = 0; row < 6; row++) { + for (col = 0; col < 6; col++) { + cpy[row][col] = a[row][col]; + } + /* set up the go_real pointer array that ludcmp likes */ + cpyptr[row] = cpy[row]; + } + + /* convert the copy to its LU decomposition */ + retval = ludcmp(cpyptr, scratchrow, 6, index, &d); + if (GO_RESULT_OK != retval) return retval; + + /* backsubstitute a column with a 1 in it to get the inverse */ + for (col = 0; col < 6; col++) { + for (row = 0; row < 6; row++) { + v[row] = 0.0; + } + v[col] = 1.0; + retval = lubksb(cpyptr, 6, index, v); + if (GO_RESULT_OK != retval) return retval; + for (row = 0; row < 6; row++) { + ainv[row][col] = v[row]; + } + } + + return GO_RESULT_OK; +} + +int go_mat6_mat6_mult(const go_real a[6][6], + const go_real b[6][6], + go_real axb[6][6]) +{ + go_real work[6][6]; + go_integer i, j, k; + + for (i = 0; i < 6; i++) { + for (j = 0; j < 6; j++) { + work[i][j] = 0; + for (k = 0; k < 6; k++) { + work[i][j] += a[i][k] * b[k][j]; + } + } + } + + for (i = 0; i < 6; i++) + for (j = 0; j < 6; j++) + axb[i][j] = work[i][j]; + + return GO_RESULT_OK; +} + +int go_mat6_vec6_mult(const go_real a[6][6], + const go_real v[6], + go_real axv[6]) +{ + go_real work[6]; + go_integer i, j; + + for (i = 0; i < 6; i++) { + work[i] = 0; + for (j = 0; j < 6; j++) { + work[i] += a[i][j] * v[j]; + } + } + + for (i = 0; i < 6; i++) + axv[i] = work[i]; + + return GO_RESULT_OK; +} + +/* recall: */ +/* | m.x.x m.y.x m.z.x | */ +/* M = | m.x.y m.y.y m.z.y | */ +/* | m.x.z m.y.z m.z.z | */ + +int go_dh_pose_convert(const go_dh * dh, go_pose * p) +{ + go_hom h; + go_real sth, cth; /* sin, cos theta[i] */ + go_real sal, cal; /* sin, cos alpha[i-1] */ + + pm_sincos(dh->theta, &sth, &cth); + pm_sincos(dh->alpha, &sal, &cal); + + h.rot.x.x = cth, h.rot.y.x = -sth, h.rot.z.x = 0.0; + h.rot.x.y = sth*cal, h.rot.y.y = cth*cal, h.rot.z.y = -sal; + h.rot.x.z = sth*sal, h.rot.y.z = cth*sal, h.rot.z.z = cal; + + h.tran.x = dh->a; + h.tran.y = -sal*dh->d; + h.tran.z = cal*dh->d; + + return go_hom_pose_convert(&h, p); +} + +int go_pose_dh_convert(const go_pose * ph, go_dh * dh) +{ + go_hom h; + + go_pose_hom_convert(ph, &h); + + dh->a = h.tran.x; + dh->alpha = -atan2(h.rot.z.y, h.rot.z.z); + dh->theta = -atan2(h.rot.y.x, h.rot.x.x); + if (GO_ROT_SMALL(dh->alpha)) { + dh->d = h.tran.z / cos(dh->alpha); + } else { + dh->d = -h.tran.y / sin(dh->alpha); + } + + return GO_RESULT_OK; +} + +int go_link_joint_set(const go_link * link, go_real joint, go_link * linkout) +{ + go_pose pose; + go_rvec rvec; + int retval; + + linkout->type = link->type; + linkout->quantity = link->quantity; + + if (GO_LINK_DH == link->type) { + linkout->u.dh.a = link->u.dh.a; + linkout->u.dh.alpha = link->u.dh.alpha; + if (GO_QUANTITY_LENGTH == link->quantity) { + linkout->u.dh.d = joint; + linkout->u.dh.theta = link->u.dh.theta; + } else { + linkout->u.dh.d = link->u.dh.d; + linkout->u.dh.theta = joint; + } + return GO_RESULT_OK; + } + + if (GO_LINK_PP == link->type) { + pose = go_pose_identity(); + if (GO_QUANTITY_LENGTH == link->quantity) { + pose.tran.z = joint; + return go_pose_pose_mult(&link->u.pp.pose, &pose, &linkout->u.pp.pose); + } + /* else revolute */ + rvec.x = 0, rvec.y = 0, rvec.z = joint; /* rot(Z,joint) */ + retval = go_rvec_quat_convert(&rvec, &pose.rot); + if (GO_RESULT_OK != retval) return retval; + return go_pose_pose_mult(&link->u.pp.pose, &pose, &linkout->u.pp.pose); + } + + if (GO_LINK_PK == link->type) { + /* + Our PK type is always a length joint, so link->quantity must + be GO_QUANTITY_LENGTH. One day we may be able to handle revolute + parallel joints, but we can't now. Let's fail if we ever get + a revolute joint. + FIXME-- add revolute joints to PKMs. + */ + if (GO_QUANTITY_LENGTH != link->quantity) { + return GO_RESULT_IMPL_ERROR; + } + /* else we're a prismatic joint */ + linkout->u.pk.base = link->u.pk.base; + linkout->u.pk.platform = link->u.pk.platform; + linkout->u.pk.d = joint; + return GO_RESULT_OK; + } + + /* else not a recognized link type */ + return GO_RESULT_ERROR; +} + +/* this only works for serial-link manipulators */ +int go_link_pose_build(const go_link * link_params, go_integer num, go_pose * pose) +{ + go_pose p; + go_integer link; + + *pose = go_pose_identity(); + + for (link = 0; link < num; link++) { + if (GO_LINK_DH == link_params[link].type) { + go_dh_pose_convert(&link_params[link].u.dh, &p); + go_pose_pose_mult(pose, &p, pose); + } else if (GO_LINK_PP == link_params[link].type) { + go_pose_pose_mult(pose, &link_params[link].u.pp.pose, pose); + } else { + return GO_RESULT_ERROR; + } + } + + return GO_RESULT_OK; +} diff --git a/wasm-port/vendor/linuxcnc/src/libnml/posemath/posemath.cc b/wasm-port/vendor/linuxcnc/src/libnml/posemath/posemath.cc new file mode 100644 index 0000000..fdd4511 --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/libnml/posemath/posemath.cc @@ -0,0 +1,1411 @@ +/******************************************************************** +* Description: posemath.cc +* C++ definitions for pose math library data types and manipulation +* functions. +* +* Derived from a work by Fred Proctor & Will Shackleford +* +* Author: +* License: LGPL Version 2 +* System: Linux +* +* Copyright (c) 2004 All rights reserved. +* +* Last change: +********************************************************************/ + +#include "posemath.h" + +#ifdef PM_PRINT_ERROR +#define PM_DEBUG // need debug with printing +#endif + +// place to reference arrays when bounds are exceeded +static double noElement = 0.0; +static PM_CARTESIAN *noCart = 0; + + +// PM_CARTESIAN class + +PM_CARTESIAN::PM_CARTESIAN(double _x, double _y, double _z) +{ + x = _x; + y = _y; + z = _z; +} + +PM_CARTESIAN::PM_CARTESIAN(PM_CONST PM_CYLINDRICAL PM_REF c) +{ + PmCylindrical cyl; + PmCartesian cart; + + toCyl(c, &cyl); + pmCylCartConvert(&cyl, &cart); + toCart(cart, this); +} + +PM_CARTESIAN::PM_CARTESIAN(PM_CONST PM_SPHERICAL PM_REF s) +{ + PmSpherical sph; + PmCartesian cart; + + toSph(s, &sph); + pmSphCartConvert(&sph, &cart); + toCart(cart, this); +} + +double &PM_CARTESIAN::operator [] (int n) { + switch (n) { + case 0: + return x; + case 1: + return y; + case 2: + return z; + default: + return noElement; // need to return a double & + } +} + +PM_CARTESIAN & PM_CARTESIAN::operator -= (const PM_CARTESIAN &o) { + x-=o.x; + y-=o.y; + z-=o.z; + return *this; +} +PM_CARTESIAN & PM_CARTESIAN::operator += (const PM_CARTESIAN &o) { + x+=o.x; + y+=o.y; + z+=o.z; + return *this; +} +/* +const PM_CARTESIAN PM_CARTESIAN::operator+(const PM_CARTESIAN &o) const { + PM_CARTESIAN result = *this; + result += o; + return result; +} + +const PM_CARTESIAN PM_CARTESIAN::operator-(const PM_CARTESIAN &o) const { + PM_CARTESIAN result = *this; + result -= o; + return result; +} +*/ + +PM_CARTESIAN & PM_CARTESIAN::operator *= (double o) +{ + x*=o; + y*=o; + z*=o; + return *this; + +} + +PM_CARTESIAN & PM_CARTESIAN::operator /= (double o) +{ + x/=o; + y/=o; + z/=o; + return *this; +} + + +#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS +PM_CARTESIAN::PM_CARTESIAN(PM_CCONST PM_CARTESIAN & v) +{ + x = v.x; + y = v.y; + z = v.z; +} +#endif + +// PM_SPHERICAL + +PM_SPHERICAL::PM_SPHERICAL(double _theta, double _phi, double _r) +{ + theta = _theta; + phi = _phi; + r = _r; +} + +PM_SPHERICAL::PM_SPHERICAL(PM_CONST PM_CARTESIAN PM_REF v) +{ + PmCartesian cart; + PmSpherical sph; + + toCart(v, &cart); + pmCartSphConvert(&cart, &sph); + toSph(sph, this); +} + +PM_SPHERICAL::PM_SPHERICAL(PM_CONST PM_CYLINDRICAL PM_REF c) +{ + PmCylindrical cyl; + PmSpherical sph; + + toCyl(c, &cyl); + pmCylSphConvert(&cyl, &sph); + toSph(sph, this); +} + +double &PM_SPHERICAL::operator [] (int n) { + switch (n) { + case 0: + return theta; + case 1: + return phi; + case 2: + return r; + default: + return noElement; // need to return a double & + } +} + +#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS +PM_SPHERICAL::PM_SPHERICAL(PM_CCONST PM_SPHERICAL & s) +{ + theta = s.theta; + phi = s.phi; + r = s.r; +} +#endif +// PM_CYLINDRICAL + +PM_CYLINDRICAL::PM_CYLINDRICAL(double _theta, double _r, double _z) +{ + theta = _theta; + r = _r; + z = _z; +} + +PM_CYLINDRICAL::PM_CYLINDRICAL(PM_CONST PM_CARTESIAN PM_REF v) +{ + PmCartesian cart; + PmCylindrical cyl; + + toCart(v, &cart); + pmCartCylConvert(&cart, &cyl); + toCyl(cyl, this); +} + +PM_CYLINDRICAL::PM_CYLINDRICAL(PM_CONST PM_SPHERICAL PM_REF s) +{ + PmSpherical sph; + PmCylindrical cyl; + + toSph(s, &sph); + pmSphCylConvert(&sph, &cyl); + toCyl(cyl, this); +} + +double &PM_CYLINDRICAL::operator [] (int n) { + switch (n) { + case 0: + return theta; + case 1: + return r; + case 2: + return z; + default: + return noElement; // need to return a double & + } +} + +#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS +PM_CYLINDRICAL::PM_CYLINDRICAL(PM_CCONST PM_CYLINDRICAL & c) +{ + theta = c.theta; + r = c.r; + z = c.z; +} +#endif +// PM_ROTATION_VECTOR + +PM_ROTATION_VECTOR::PM_ROTATION_VECTOR(double _s, double _x, + double _y, double _z) +{ + PmRotationVector rv; + + rv.s = _s; + rv.x = _x; + rv.y = _y; + rv.z = _z; + + pmRotNorm(&rv, &rv); + toRot(rv, this); +} + +PM_ROTATION_VECTOR::PM_ROTATION_VECTOR(PM_CONST PM_QUATERNION PM_REF q) +{ + PmQuaternion quat; + PmRotationVector rv; + + toQuat(q, &quat); + pmQuatRotConvert(&quat, &rv); + toRot(rv, this); +} + +double &PM_ROTATION_VECTOR::operator [] (int n) { + switch (n) { + case 0: + return s; + case 1: + return x; + case 2: + return y; + case 3: + return z; + default: + return noElement; // need to return a double & + } +} + +#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS +PM_ROTATION_VECTOR::PM_ROTATION_VECTOR(PM_CCONST PM_ROTATION_VECTOR & r) +{ + s = r.s; + x = r.x; + y = r.y; + z = r.z; +} +#endif +// PM_ROTATION_MATRIX class + +// ctors/dtors + +PM_ROTATION_MATRIX::PM_ROTATION_MATRIX(double xx, double xy, double xz, + double yx, double yy, double yz, double zx, double zy, double zz) +{ + x.x = xx; + x.y = xy; + x.z = xz; + + y.x = yx; + y.y = yy; + y.z = yz; + + z.x = zx; + z.y = zy; + z.z = zz; + + /*! \todo FIXME-- need a matrix orthonormalization function pmMatNorm() */ +} + +PM_ROTATION_MATRIX::PM_ROTATION_MATRIX(const PM_CARTESIAN& _x, const PM_CARTESIAN& _y, const PM_CARTESIAN& _z) + : x(_x), y(_y), z(_z) +{ +} + +PM_ROTATION_MATRIX::PM_ROTATION_MATRIX(PM_CONST PM_ROTATION_VECTOR PM_REF v) +{ + PmRotationVector rv; + PmRotationMatrix mat; + + toRot(v, &rv); + pmRotMatConvert(&rv, &mat); + toMat(mat, this); +} + +PM_ROTATION_MATRIX::PM_ROTATION_MATRIX(PM_CONST PM_QUATERNION PM_REF q) +{ + PmQuaternion quat; + PmRotationMatrix mat; + + toQuat(q, &quat); + pmQuatMatConvert(&quat, &mat); + toMat(mat, this); +} + +PM_ROTATION_MATRIX::PM_ROTATION_MATRIX(PM_CONST PM_RPY PM_REF rpy) +{ + PmRpy _rpy; + PmRotationMatrix mat; + + toRpy(rpy, &_rpy); + pmRpyMatConvert(&_rpy, &mat); + toMat(mat, this); +} + +PM_ROTATION_MATRIX::PM_ROTATION_MATRIX(PM_CONST PM_EULER_ZYZ PM_REF zyz) +{ + PmEulerZyz _zyz; + PmRotationMatrix mat; + + toEulerZyz(zyz, &_zyz); + pmZyzMatConvert(&_zyz, &mat); + toMat(mat, this); +} + +PM_ROTATION_MATRIX::PM_ROTATION_MATRIX(PM_CONST PM_EULER_ZYX PM_REF zyx) +{ + PmEulerZyx _zyx; + PmRotationMatrix mat; + + toEulerZyx(zyx, &_zyx); + pmZyxMatConvert(&_zyx, &mat); + toMat(mat, this); +} + +// operators + +PM_CARTESIAN & PM_ROTATION_MATRIX::operator [](int n) { + switch (n) { + case 0: + return x; + case 1: + return y; + case 2: + return z; + default: + if (0 == noCart) { + noCart = new PM_CARTESIAN(0.0, 0.0, 0.0); + } + return (*noCart); // need to return a PM_CARTESIAN & + } +} + +#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS +PM_ROTATION_MATRIX::PM_ROTATION_MATRIX(PM_CCONST PM_ROTATION_MATRIX & m) + : x(m.x), y(m.y), z(m.z) +{ +} +#endif +// PM_QUATERNION class + +PM_QUATERNION::PM_QUATERNION(double _s, double _x, double _y, double _z) +{ + PmQuaternion quat; + + quat.s = _s; + quat.x = _x; + quat.y = _y; + quat.z = _z; + + pmQuatNorm(&quat, &quat); + + s = quat.s; + x = quat.x; + y = quat.y; + z = quat.z; +} + +PM_QUATERNION::PM_QUATERNION(PM_CONST PM_ROTATION_VECTOR PM_REF v) +{ + PmRotationVector rv; + PmQuaternion quat; + + toRot(v, &rv); + pmRotQuatConvert(&rv, &quat); + toQuat(quat, this); +} + +PM_QUATERNION::PM_QUATERNION(PM_CONST PM_ROTATION_MATRIX PM_REF m) +{ + PmRotationMatrix mat; + PmQuaternion quat; + + toMat(m, &mat); + pmMatQuatConvert(&mat, &quat); + toQuat(quat, this); +} + +PM_QUATERNION::PM_QUATERNION(PM_CONST PM_EULER_ZYZ PM_REF zyz) +{ + PmEulerZyz _zyz; + PmQuaternion quat; + + toEulerZyz(zyz, &_zyz); + pmZyzQuatConvert(&_zyz, &quat); + toQuat(quat, this); +} + +PM_QUATERNION::PM_QUATERNION(PM_CONST PM_EULER_ZYX PM_REF zyx) +{ + PmEulerZyx _zyx; + PmQuaternion quat; + + toEulerZyx(zyx, &_zyx); + pmZyxQuatConvert(&_zyx, &quat); + toQuat(quat, this); +} + +PM_QUATERNION::PM_QUATERNION(PM_CONST PM_RPY PM_REF rpy) +{ + PmRpy _rpy; + PmQuaternion quat; + + toRpy(rpy, &_rpy); + pmRpyQuatConvert(&_rpy, &quat); + toQuat(quat, this); +} + +PM_QUATERNION::PM_QUATERNION(PM_AXIS _axis, double _angle) +{ + PmQuaternion quat; + + pmAxisAngleQuatConvert((PmAxis) _axis, _angle, &quat); + toQuat(quat, this); +} + +void PM_QUATERNION::axisAngleMult(PM_AXIS _axis, double _angle) +{ + PmQuaternion quat; + + toQuat((*this), &quat); + pmQuatAxisAngleMult(&quat, (PmAxis) _axis, _angle, &quat); + toQuat(quat, this); +} + +double &PM_QUATERNION::operator [] (int n) { + switch (n) { + case 0: + return s; + case 1: + return x; + case 2: + return y; + case 3: + return z; + default: + return noElement; // need to return a double & + } +} +#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS +PM_QUATERNION::PM_QUATERNION(PM_CCONST PM_QUATERNION & q) +{ + s = q.s; + x = q.x; + y = q.y; + z = q.z; +} +#endif + +// PM_EULER_ZYZ class + +PM_EULER_ZYZ::PM_EULER_ZYZ(double _z, double _y, double _zp) +{ + z = _z; + y = _y; + zp = _zp; +} + +PM_EULER_ZYZ::PM_EULER_ZYZ(PM_CONST PM_QUATERNION PM_REF q) +{ + PmQuaternion quat; + PmEulerZyz zyz; + + toQuat(q, &quat); + pmQuatZyzConvert(&quat, &zyz); + toEulerZyz(zyz, this); +} + +PM_EULER_ZYZ::PM_EULER_ZYZ(PM_CONST PM_ROTATION_MATRIX PM_REF m) +{ + PmRotationMatrix mat; + PmEulerZyz zyz; + + toMat(m, &mat); + pmMatZyzConvert(&mat, &zyz); + toEulerZyz(zyz, this); +} + +double &PM_EULER_ZYZ::operator [] (int n) { + switch (n) { + case 0: + return z; + case 1: + return y; + case 2: + return zp; + default: + return noElement; // need to return a double & + } +} + +#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS +PM_EULER_ZYZ::PM_EULER_ZYZ(PM_CCONST PM_EULER_ZYZ & zyz) +{ + z = zyz.z; + y = zyz.y; + zp = zyz.zp; +} +#endif + +// PM_EULER_ZYX class + +PM_EULER_ZYX::PM_EULER_ZYX(double _z, double _y, double _x) +{ + z = _z; + y = _y; + x = _x; +} + +PM_EULER_ZYX::PM_EULER_ZYX(PM_CONST PM_QUATERNION PM_REF q) +{ + PmQuaternion quat; + PmEulerZyx zyx; + + toQuat(q, &quat); + pmQuatZyxConvert(&quat, &zyx); + toEulerZyx(zyx, this); +} + +PM_EULER_ZYX::PM_EULER_ZYX(PM_CONST PM_ROTATION_MATRIX PM_REF m) +{ + PmRotationMatrix mat; + PmEulerZyx zyx; + + toMat(m, &mat); + pmMatZyxConvert(&mat, &zyx); + toEulerZyx(zyx, this); +} + +double &PM_EULER_ZYX::operator [] (int n) { + switch (n) { + case 0: + return z; + case 1: + return y; + case 2: + return x; + default: + return noElement; // need to return a double & + } +} + +#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS +PM_EULER_ZYX::PM_EULER_ZYX(PM_CCONST PM_EULER_ZYX & zyx) +{ + z = zyx.z; + y = zyx.y; + x = zyx.x; +} +#endif +// PM_RPY class + +#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS +PM_RPY::PM_RPY(PM_CCONST PM_RPY & rpy) +{ + r = rpy.r; + p = rpy.p; + y = rpy.y; +} +#endif + +PM_RPY::PM_RPY(double _r, double _p, double _y) +{ + r = _r; + p = _p; + y = _y; +} + +PM_RPY::PM_RPY(PM_CONST PM_QUATERNION PM_REF q) +{ + PmQuaternion quat; + PmRpy rpy; + + toQuat(q, &quat); + pmQuatRpyConvert(&quat, &rpy); + toRpy(rpy, this); +} + +PM_RPY::PM_RPY(PM_CONST PM_ROTATION_MATRIX PM_REF m) +{ + PmRotationMatrix mat; + PmRpy rpy; + + toMat(m, &mat); + pmMatRpyConvert(&mat, &rpy); + toRpy(rpy, this); +} + +double &PM_RPY::operator [] (int n) { + switch (n) { + case 0: + return r; + case 1: + return p; + case 2: + return y; + default: + return noElement; // need to return a double & + } +} + +// PM_POSE class + +PM_POSE::PM_POSE(const PM_CARTESIAN& v, const PM_QUATERNION& q) +{ + tran.x = v.x; + tran.y = v.y; + tran.z = v.z; + rot.s = q.s; + rot.x = q.x; + rot.y = q.y; + rot.z = q.z; +} + +PM_POSE::PM_POSE(double x, double y, double z, + double s, double sx, double sy, double sz) +{ + tran.x = x; + tran.y = y; + tran.z = z; + rot.s = s; + rot.x = sx; + rot.y = sy; + rot.z = sz; +} + +PM_POSE::PM_POSE(PM_CONST PM_HOMOGENEOUS PM_REF h) +{ + PmHomogeneous hom; + PmPose pose; + + toHom(h, &hom); + pmHomPoseConvert(&hom, &pose); + toPose(pose, this); +} + +double &PM_POSE::operator [] (int n) { + switch (n) { + case 0: + return tran.x; + case 1: + return tran.y; + case 2: + return tran.z; + case 3: + return rot.s; + case 4: + return rot.x; + case 5: + return rot.y; + case 6: + return rot.z; + default: + return noElement; // need to return a double & + } +} + +#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS +PM_POSE::PM_POSE(PM_CCONST PM_POSE & p) + : tran(p.tran), rot(p.rot) +{ +} +#endif +// PM_HOMOGENEOUS class + +PM_HOMOGENEOUS::PM_HOMOGENEOUS(const PM_CARTESIAN& v, const PM_ROTATION_MATRIX& m) + : tran(v), rot(m) +{ +} + +PM_HOMOGENEOUS::PM_HOMOGENEOUS(PM_CONST PM_POSE PM_REF p) +{ + PmPose pose; + PmHomogeneous hom; + + toPose(p, &pose); + pmPoseHomConvert(&pose, &hom); + toHom(hom, this); +} + +PM_CARTESIAN & PM_HOMOGENEOUS::operator [](int n) { + // if it is a rotation vector, stuff 0 as default bottom + // if it is a translation vector, stuff 1 as default bottom + + switch (n) { + case 0: + noElement = 0.0; + return rot.x; + case 1: + noElement = 0.0; + return rot.y; + case 2: + noElement = 0.0; + return rot.z; + case 3: + noElement = 1.0; + return tran; + default: + if (0 == noCart) { + noCart = new PM_CARTESIAN(0.0, 0.0, 0.0); + } + return (*noCart); // need to return a PM_CARTESIAN & + } +} + +#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS +PM_HOMOGENEOUS::PM_HOMOGENEOUS(PM_CCONST PM_HOMOGENEOUS & h) + : tran(h.tran), rot(h.rot) +{ +} +#endif + +// PM_LINE class + +#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS +PM_LINE::PM_LINE(PM_CCONST PM_LINE & l) + : start(l.start), end(l.end), uVec(l.uVec) +{ +} +#endif + +int PM_LINE::init(const PM_POSE& start, const PM_POSE& end) +{ + PmLine _line; + PmPose _start, _end; + int retval; + + toPose(start, &_start); + toPose(end, &_end); + + retval = pmLineInit(&_line, &_start, &_end); + + toLine(_line, this); + + return retval; +} + +int PM_LINE::point(double len, PM_POSE * point) +{ + PmLine _line; + PmPose _point; + int retval; + + toLine(*this, &_line); + + retval = pmLinePoint(&_line, len, &_point); + + toPose(_point, point); + + return retval; +} + +// PM_CIRCLE class + +#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS +PM_CIRCLE::PM_CIRCLE(PM_CCONST PM_CIRCLE & c) + : center(c.center), normal(c.normal), + rTan(c.rTan), rPerp(c.rPerp), rHelix(c.rHelix), + radius(c.radius), angle(c.angle), spiral(c.spiral) +{ +} +#endif + +int PM_CIRCLE::init(const PM_POSE& start, const PM_POSE& end, + const PM_CARTESIAN& center, const PM_CARTESIAN& normal, int turn) +{ + PmCircle _circle; + PmPose _start, _end; + PmCartesian _center, _normal; + int retval; + + toPose(start, &_start); + toPose(end, &_end); + toCart(center, &_center); + toCart(normal, &_normal); + + retval = pmCircleInit(&_circle, &_start.tran, &_end.tran, &_center, &_normal, turn); + + toCircle(_circle, this); + + return retval; +} + +int PM_CIRCLE::point(double angle, PM_POSE * point) +{ + PmCircle _circle; + PmPose _point; + int retval; + + toCircle(*this, &_circle); + + retval = pmCirclePoint(&_circle, angle, &_point.tran); + + toPose(_point, point); + + return retval; +} + +// overloaded external functions + +// dot + +double dot(const PM_CARTESIAN &v1, const PM_CARTESIAN &v2) +{ + double d; + PmCartesian _v1, _v2; + + toCart(v1, &_v1); + toCart(v2, &_v2); + + pmCartCartDot(&_v1, &_v2, &d); + + return d; +} + +// cross + +PM_CARTESIAN cross(const PM_CARTESIAN &v1, const PM_CARTESIAN &v2) +{ + PM_CARTESIAN ret; + PmCartesian _v1, _v2, _v3; + + toCart(v1, &_v1); + toCart(v2, &_v2); + + pmCartCartCross(&_v1, &_v2, &_v3); + + toCart(_v3, &ret); + + return ret; +} + +//unit + +PM_CARTESIAN unit(const PM_CARTESIAN &v) +{ + PM_CARTESIAN vout; + PmCartesian _v; + + toCart(v, &_v); + + pmCartUnitEq(&_v); + + toCart(_v, &vout); + + return vout; +} + +/*! \todo Another #if 0 */ +#if 0 + +PM_CARTESIAN norm(PM_CARTESIAN v) +{ + PM_CARTESIAN vout; + PmCartesian _v; + + toCart(v, &_v); + + pmCartNorm(&_v, &_v); + + toCart(_v, &vout); + + return vout; +} + +PM_QUATERNION norm(PM_QUATERNION q) +{ + PM_QUATERNION qout; + PmQuaternion _q; + + toQuat(q, &_q); + pmQuatNorm(_q, &_q); + + toQuat(_q, &qout); + + return qout; +} + +PM_ROTATION_VECTOR norm(PM_ROTATION_VECTOR r) +{ + PM_ROTATION_VECTOR rout; + PmRotationVector _r; + + toRot(r, &_r); + + pmRotNorm(_r, &_r); + + toRot(_r, &rout); + + return rout; +} + +PM_ROTATION_MATRIX norm(PM_ROTATION_MATRIX m) +{ + PM_ROTATION_MATRIX mout; + PmRotationMatrix _m; + + toMat(m, &_m); + + pmMatNorm(_m, &_m); + + toMat(_m, &mout); + + return mout; +} +#endif + +// isNorm + +int isNorm(const PM_CARTESIAN& v) +{ + PmCartesian _v; + + toCart(v, &_v); + + return pmCartIsNorm(&_v); +} + +int isNorm(const PM_QUATERNION& q) +{ + PmQuaternion _q; + + toQuat(q, &_q); + + return pmQuatIsNorm(&_q); +} + +int isNorm(const PM_ROTATION_VECTOR& r) +{ + PmRotationVector _r; + + toRot(r, &_r); + + return pmRotIsNorm(&_r); +} + +int isNorm(const PM_ROTATION_MATRIX& m) +{ + PmRotationMatrix _m; + + toMat(m, &_m); + + return pmMatIsNorm(&_m); +} + +// mag + +double mag(const PM_CARTESIAN &v) +{ + double ret; + PmCartesian _v; + + toCart(v, &_v); + + pmCartMag(&_v, &ret); + + return ret; +} + +// disp + +double disp(const PM_CARTESIAN &v1, const PM_CARTESIAN &v2) +{ + double ret; + PmCartesian _v1, _v2; + + toCart(v1, &_v1); + toCart(v2, &_v2); + + pmCartCartDisp(&_v1, &_v2, &ret); + + return ret; +} + +// inv + +PM_CARTESIAN inv(const PM_CARTESIAN &v) +{ + PM_CARTESIAN ret; + PmCartesian _v; + + toCart(v, &_v); + + pmCartInv(&_v, &_v); + + toCart(_v, &ret); + + return ret; +} + +PM_ROTATION_MATRIX inv(const PM_ROTATION_MATRIX &m) +{ + PM_ROTATION_MATRIX ret; + PmRotationMatrix _m; + + toMat(m, &_m); + + pmMatInv(&_m, &_m); + + toMat(_m, &ret); + + return ret; +} + +PM_QUATERNION inv(const PM_QUATERNION &q) +{ + PM_QUATERNION ret; + PmQuaternion _q; + + toQuat(q, &_q); + + pmQuatInv(&_q, &_q); + + toQuat(_q, &ret); + + return ret; +} + +PM_POSE inv(const PM_POSE &p) +{ + PM_POSE ret; + PmPose _p; + + toPose(p, &_p); + + pmPoseInv(&_p, &_p); + + toPose(_p, &ret); + + return ret; +} + +PM_HOMOGENEOUS inv(const PM_HOMOGENEOUS &h) +{ + PM_HOMOGENEOUS ret; + PmHomogeneous _h; + + toHom(h, &_h); + + pmHomInv(&_h, &_h); + + toHom(_h, &ret); + + return ret; +} + +// project + +PM_CARTESIAN proj(const PM_CARTESIAN &v1, PM_CARTESIAN &v2) +{ + PM_CARTESIAN ret; + PmCartesian _v1, _v2; + + toCart(v1, &_v1); + toCart(v2, &_v2); + + pmCartCartProj(&_v1, &_v2, &_v1); + + toCart(_v1, &ret); + + return ret; +} + +// overloaded arithmetic operators + +PM_CARTESIAN operator +(const PM_CARTESIAN &v) +{ + return v; +} + +PM_CARTESIAN operator -(const PM_CARTESIAN &v) +{ + PM_CARTESIAN ret; + + ret.x = -v.x; + ret.y = -v.y; + ret.z = -v.z; + + return ret; +} + +PM_QUATERNION operator +(const PM_QUATERNION &q) +{ + return q; +} + +PM_QUATERNION operator -(const PM_QUATERNION &q) +{ + PM_QUATERNION ret; + PmQuaternion _q; + + toQuat(q, &_q); + + pmQuatInv(&_q, &_q); + + toQuat(_q, &ret); + + return ret; +} + +PM_POSE operator +(const PM_POSE &p) +{ + return p; +} + +PM_POSE operator -(const PM_POSE &p) +{ + PM_POSE ret; + PmPose _p; + + toPose(p, &_p); + + pmPoseInv(&_p, &_p); + + toPose(_p, &ret); + + return ret; +} + +int operator ==(const PM_CARTESIAN &v1, const PM_CARTESIAN &v2) +{ + PmCartesian _v1, _v2; + + toCart(v1, &_v1); + toCart(v2, &_v2); + + return pmCartCartCompare(&_v1, &_v2); +} + +int operator ==(const PM_QUATERNION &q1, PM_QUATERNION &q2) +{ + PmQuaternion _q1, _q2; + + toQuat(q1, &_q1); + toQuat(q2, &_q2); + + return pmQuatQuatCompare(&_q1, &_q2); +} + +int operator ==(const PM_POSE &p1, const PM_POSE &p2) +{ + PmPose _p1, _p2; + + toPose(p1, &_p1); + toPose(p2, &_p2); + + return pmPosePoseCompare(&_p1, &_p2); +} + +int operator !=(const PM_CARTESIAN &v1, const PM_CARTESIAN &v2) +{ + PmCartesian _v1, _v2; + + toCart(v1, &_v1); + toCart(v2, &_v2); + + return !pmCartCartCompare(&_v1, &_v2); +} + +int operator !=(const PM_QUATERNION &q1, const PM_QUATERNION &q2) +{ + PmQuaternion _q1, _q2; + + toQuat(q1, &_q1); + toQuat(q2, &_q2); + + return !pmQuatQuatCompare(&_q1, &_q2); +} + +int operator !=(const PM_POSE &p1, const PM_POSE &p2) +{ + PmPose _p1, _p2; + + toPose(p1, &_p1); + toPose(p2, &_p2); + + return !pmPosePoseCompare(&_p1, &_p2); +} + +PM_CARTESIAN operator +(PM_CARTESIAN v1, const PM_CARTESIAN &v2) +{ + v1 += v2; + return v1; +} + +PM_CARTESIAN operator -(PM_CARTESIAN v1, const PM_CARTESIAN &v2) +{ + v1 -= v2; + return v1; +} + +PM_CARTESIAN operator *(PM_CARTESIAN v, double s) +{ + v *= s; + return v; +} + +PM_CARTESIAN operator *(double s, PM_CARTESIAN v) +{ + v *= s; + return v; +} + +PM_CARTESIAN operator /(const PM_CARTESIAN &v, double s) +{ + PM_CARTESIAN ret; + +#ifdef PM_DEBUG + if (s == 0.0) { +#ifdef PM_PRINT_ERROR + pmPrintError("PM_CARTESIAN::operator / : divide by 0\n"); +#endif + pmErrno = PM_DIV_ERR; + return ret; + } +#endif + + ret.x = v.x / s; + ret.y = v.y / s; + ret.z = v.z / s; + + return ret; +} + +PM_QUATERNION operator *(double s, const PM_QUATERNION &q) +{ + PM_QUATERNION qout; + PmQuaternion _q; + + toQuat(q, &_q); + + pmQuatScalMult(&_q, s, &_q); + + toQuat(_q, &qout); + + return qout; +} + +PM_QUATERNION operator *(const PM_QUATERNION &q, double s) +{ + PM_QUATERNION qout; + PmQuaternion _q; + + toQuat(q, &_q); + + pmQuatScalMult(&_q, s, &_q); + + toQuat(_q, &qout); + + return qout; +} + +PM_QUATERNION operator /(const PM_QUATERNION &q, double s) +{ + PM_QUATERNION qout; + PmQuaternion _q; + + toQuat(q, &_q); + +#ifdef PM_DEBUG + if (s == 0.0) { +#ifdef PM_PRINT_ERROR + pmPrintError("Divide by 0 in operator /\n"); +#endif + pmErrno = PM_NORM_ERR; + +/*! \todo Another #if 0 */ +#if 0 + // g++/gcc versions 2.8.x and 2.9.x + // will complain that the call to PM_QUATERNION(PM_QUATERNION) is + // ambiguous. (2.7.x and some others allow it) + return qout = + PM_QUATERNION((double) 0, (double) 0, (double) 0, (double) 0); +#else + + PmQuaternion quat; + + quat.s = 0; + quat.x = 0; + quat.y = 0; + quat.z = 0; + + pmQuatNorm(&quat, &quat); + + qout.s = quat.s; + qout.x = quat.x; + qout.y = quat.y; + qout.z = quat.z; + return qout; +#endif + + } +#endif + + pmQuatScalMult(&_q, 1.0 / s, &_q); + toQuat(_q, &qout); + + pmErrno = 0; + return qout; +} + +PM_CARTESIAN operator *(const PM_QUATERNION &q, const PM_CARTESIAN &v) +{ + PM_CARTESIAN vout; + PmQuaternion _q; + PmCartesian _v; + + toQuat(q, &_q); + toCart(v, &_v); + + pmQuatCartMult(&_q, &_v, &_v); + + toCart(_v, &vout); + + return vout; +} + +PM_QUATERNION operator *(const PM_QUATERNION &q1, const PM_QUATERNION &q2) +{ + PM_QUATERNION ret; + PmQuaternion _q1, _q2; + + toQuat(q1, &_q1); + toQuat(q2, &_q2); + + pmQuatQuatMult(&_q1, &_q2, &_q1); + + toQuat(_q1, &ret); + + return ret; +} + +PM_ROTATION_MATRIX operator *(const PM_ROTATION_MATRIX &m1, const PM_ROTATION_MATRIX &m2) +{ + PM_ROTATION_MATRIX ret; + PmRotationMatrix _m1, _m2; + + toMat(m1, &_m1); + toMat(m2, &_m2); + + pmMatMatMult(&_m1, &_m2, &_m1); + + toMat(_m1, &ret); + + return ret; +} + +PM_POSE operator *(const PM_POSE &p1, const PM_POSE &p2) +{ + PM_POSE ret; + PmPose _p1, _p2; + + toPose(p1, &_p1); + toPose(p2, &_p2); + + pmPosePoseMult(&_p1, &_p2, &_p1); + + toPose(_p1, &ret); + + return ret; +} + +PM_CARTESIAN operator *(const PM_POSE &p, const PM_CARTESIAN &v) +{ + PM_CARTESIAN ret; + PmPose _p; + PmCartesian _v; + + toPose(p, &_p); + toCart(v, &_v); + + pmPoseCartMult(&_p, &_v, &_v); + + toCart(_v, &ret); + + return ret; +} diff --git a/wasm-port/vendor/linuxcnc/src/libnml/posemath/sincos.c b/wasm-port/vendor/linuxcnc/src/libnml/posemath/sincos.c new file mode 100644 index 0000000..6cf16df --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/libnml/posemath/sincos.c @@ -0,0 +1,27 @@ +/******************************************************************** +* Description: sincos.c +* +* Derived from a work by Fred Proctor & Will Shackleford +* +* Author: +* License: LGPL Version 2 +* System: Linux +* +* Copyright (c) 2004 All rights reserved. +********************************************************************/ +/* + sincos.c + + Modification history: + + 21-Jan-2004 P.C. Moved across from the original EMC source tree. +*/ + +#include +#include "sincos.h" + +void pm_sincos(double x, double *sx, double *cx) +{ + *sx = sin(x); + *cx = cos(x); +} diff --git a/wasm-port/vendor/linuxcnc/src/rtapi/rtapi_atomic.h b/wasm-port/vendor/linuxcnc/src/rtapi/rtapi_atomic.h new file mode 100644 index 0000000..e0d2049 --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/rtapi/rtapi_atomic.h @@ -0,0 +1,81 @@ +// Copyright 2015 Jeff Epler +// Copyright 2026 B.Stultiens +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. +#ifndef __LINUXCNC_RTAPI_ATOMIC_H +#define __LINUXCNC_RTAPI_ATOMIC_H + +#if defined(__cplusplus) + +// We use C++20 and that has all the atomics we need +#include + +#else // defined(__cplusplus) + +// Standard C, we require C11 or better +#if defined(__STDC_VERSION__) && __STDC_VERSION__ >= 201112L +#define RTAPI_USE_STDATOMIC +#elif defined(__GNUC__) && ((__GNUC__ << 8) | __GNUC_MINOR__) >= 0x409 +#define RTAPI_USE_STDATOMIC +#endif + +#if defined(RTAPI_USE_STDATOMIC) +#include + +#if defined(__STDC_NO_ATOMICS__) +#error "Your compiler/libc has set __STDC_NO_ATOMICS__ and atomics are required." +#endif + +#else // defined(RTAPI_USE_STDATOMIC) + +#error "Old compiler has no C11 atomics. Please upgrade your compiler to support C11 or better." + +#endif // defined(RTAPI_USE_STDATOMIC) + +#endif // defined(__cplusplus) + +/* Prefixed aliases for the C11 atomic typedefs. C++ pre-C++23 does not + expose the unqualified typedefs at global scope, so use + these names when declaring atomic fields in headers shared between C + and C++ translation units. */ +#if defined(__cplusplus) +typedef std::atomic_bool rtapi_atomic_bool; +typedef std::atomic_char rtapi_atomic_char; +typedef std::atomic_schar rtapi_atomic_schar; +typedef std::atomic_uchar rtapi_atomic_uchar; +typedef std::atomic_short rtapi_atomic_short; +typedef std::atomic_ushort rtapi_atomic_ushort; +typedef std::atomic_int rtapi_atomic_int; +typedef std::atomic_uint rtapi_atomic_uint; +typedef std::atomic_long rtapi_atomic_long; +typedef std::atomic_ulong rtapi_atomic_ulong; +typedef std::atomic_llong rtapi_atomic_llong; +typedef std::atomic_ullong rtapi_atomic_ullong; +#else +typedef atomic_bool rtapi_atomic_bool; +typedef atomic_char rtapi_atomic_char; +typedef atomic_schar rtapi_atomic_schar; +typedef atomic_uchar rtapi_atomic_uchar; +typedef atomic_short rtapi_atomic_short; +typedef atomic_ushort rtapi_atomic_ushort; +typedef atomic_int rtapi_atomic_int; +typedef atomic_uint rtapi_atomic_uint; +typedef atomic_long rtapi_atomic_long; +typedef atomic_ulong rtapi_atomic_ulong; +typedef atomic_llong rtapi_atomic_llong; +typedef atomic_ullong rtapi_atomic_ullong; +#endif + +#endif diff --git a/wasm-port/vendor/linuxcnc/src/rtapi/rtapi_bool.h b/wasm-port/vendor/linuxcnc/src/rtapi/rtapi_bool.h new file mode 100644 index 0000000..2040fc7 --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/rtapi/rtapi_bool.h @@ -0,0 +1,25 @@ +// Copyright 2014 Jeff Epler +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. +#ifndef __LINUXCNC_RTAPI_BOOL_H + +#if defined(__KERNEL__) +#include +#elif !defined(__cplusplus) +// a note in gcc's stdbool.h says "supporting in C++ is a GCC extension" +#include +#endif + +#endif diff --git a/wasm-port/vendor/linuxcnc/src/rtapi/rtapi_limits.h b/wasm-port/vendor/linuxcnc/src/rtapi/rtapi_limits.h new file mode 100644 index 0000000..6a38f5e --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/rtapi/rtapi_limits.h @@ -0,0 +1,27 @@ +/* + * Copyright (C) 2013 Jeff Epler + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + */ +#ifndef __LINUXCNC_RTAPI_LIMITS_H +#define __LINUXCNC_RTAPI_LIMITS_H + +#if defined(__KERNEL__) +#include +#else +#include +#endif + +#endif diff --git a/wasm-port/vendor/linuxcnc/src/rtapi/rtapi_slab.h b/wasm-port/vendor/linuxcnc/src/rtapi/rtapi_slab.h new file mode 100644 index 0000000..96318f1 --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/rtapi/rtapi_slab.h @@ -0,0 +1,38 @@ +// Copyright 2014 Jeff Epler +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. +#ifndef __LINUXCNC_RTAPI_SLAB_H +#define __LINUXCNC_RTAPI_SLAB_H + +#include "rtapi_gfp.h" + +#ifdef __KERNEL__ +#include + +#define rtapi_kfree kfree +#define rtapi_kmalloc kmalloc +#define rtapi_krealloc krealloc +#define rtapi_kzalloc kzalloc + +#else +#include + +#define rtapi_kfree free +#define rtapi_kmalloc(sz, flags) malloc((sz)) +#define rtapi_kzalloc(sz, flags) calloc(1,(sz)) +#define rtapi_krealloc(p, sz, flags) realloc((p), (sz)) + +#endif +#endif