✨ feat(native-planning): 添加 Rust ICSP 原生规划核心及 C# 互操作集成

* 新增 Rust 原生 ICSP 规划库 (Cargo 项目, CubicSpline 算法, FFI 接口)
* 新增 Flyshot.Native.Planning C# 互操作项目 (P/Invoke 封装)
* 更新 ControllerClientCompat 适配新的规划接口
* 添加构建脚本 (Copy-NativePlanningArtifacts.ps1)
* 新增互操作测试及一致性对比测试
* 更新解决方案文件和设计文档

native/flyshot-native-planning/Cargo.lock

@@ -0,0 +1,7 @@
+# This file is automatically @generated by Cargo.
+# It is not intended for manual editing.
+version = 4
+
+[[package]]
+name = "flyshot-native-planning"
+version = "0.1.0"

native/flyshot-native-planning/Cargo.toml

@@ -0,0 +1,13 @@
+[package]
+name = "flyshot-native-planning"
+version = "0.1.0"
+edition = "2021"
+
+[lib]
+crate-type = ["cdylib"]
+
+[profile.release]
+lto = true
+codegen-units = 1
+strip = "symbols"
+panic = "abort"

native/flyshot-native-planning/src/error.rs

@@ -0,0 +1,33 @@
+//! 原生规划错误定义。
+
+use std::fmt::{Display, Formatter};
+
+/// 表示 Rust 规划核心中可预期、可转换为 C ABI 错误码的失败。
+#[derive(Debug, Clone, PartialEq)]
+pub enum NativePlanningError {
+    /// 表示输入参数不满足算法前置条件。
+    InvalidInput(String),
+    /// 表示数值计算无法继续。
+    NumericalFailure(String),
+    /// 表示规划迭代结束后仍未满足约束。
+    NotConverged(String),
+}
+
+impl Display for NativePlanningError {
+    /// 将错误格式化为可供托管侧诊断的中文文本。
+    ///
+    /// # 参数
+    /// - `formatter`: 接收格式化文本的输出缓冲区。
+    ///
+    /// # 返回值
+    /// 格式化成功时返回 `Ok(())`，否则返回底层格式化错误。
+    fn fmt(&self, formatter: &mut Formatter<'_>) -> std::fmt::Result {
+        match self {
+            Self::InvalidInput(message) => write!(formatter, "输入非法：{message}"),
+            Self::NumericalFailure(message) => write!(formatter, "数值计算失败：{message}"),
+            Self::NotConverged(message) => write!(formatter, "ICSP 未收敛：{message}"),
+        }
+    }
+}
+
+impl std::error::Error for NativePlanningError {}

native/flyshot-native-planning/src/ffi.rs

@@ -0,0 +1,436 @@
+//! C ABI 边界定义。
+
+use std::cell::RefCell;
+use std::ffi::{c_char, CString};
+use std::panic::{catch_unwind, AssertUnwindSafe};
+use std::ptr;
+use std::slice;
+
+use crate::error::NativePlanningError;
+use crate::icsp::{plan_icsp, IcspInput, IcspOptions};
+
+/// 原生调用成功时返回的固定错误码。
+pub const FLYSHOT_SUCCESS: i32 = 0;
+/// 输入指针、维度或数值非法时返回的错误码。
+pub const FLYSHOT_ERROR_INVALID_INPUT: i32 = 1;
+/// 样条或 ICSP 数值计算无法继续时返回的错误码。
+pub const FLYSHOT_ERROR_NUMERICAL_FAILURE: i32 = 2;
+/// 迭代完成但最终 scale 仍超限时返回的错误码。
+pub const FLYSHOT_ERROR_NOT_CONVERGED: i32 = 3;
+/// FFI 捕获到未预期 panic 时返回的错误码。
+pub const FLYSHOT_ERROR_INTERNAL: i32 = 4;
+
+thread_local! {
+    /// 保存当前线程最近一次原生错误文本，指针仅在下一次原生调用前有效。
+    static LAST_ERROR_MESSAGE: RefCell<CString> = RefCell::new(CString::default());
+}
+
+/// 表示 C ABI 输入的 ICSP 迭代选项。
+#[repr(C)]
+#[derive(Debug, Clone, Copy)]
+pub struct FlyshotIcspOptions {
+    /// 收敛指标停止阈值。
+    pub threshold: f64,
+    /// 最大迭代索引，必须为非负数。
+    pub max_iterations: i32,
+    /// 非零表示启用最终 scale 检查。
+    pub enforce_final_scale: i32,
+    /// 最终 scale 允许的非负容差。
+    pub final_scale_tolerance: f64,
+}
+
+impl Default for FlyshotIcspOptions {
+    /// 返回与托管 ICSP 默认行为一致的 C ABI 选项。
+    ///
+    /// # 返回值
+    /// 可直接传入 `flyshot_icsp_plan` 的默认选项。
+    fn default() -> Self {
+        let options = IcspOptions::default();
+        Self {
+            threshold: options.threshold,
+            max_iterations: options.max_iterations as i32,
+            enforce_final_scale: i32::from(options.enforce_final_scale),
+            final_scale_tolerance: options.final_scale_tolerance,
+        }
+    }
+}
+
+/// 表示 Rust 分配并通过 C ABI 返回的 ICSP 结果。
+#[repr(C)]
+#[derive(Debug)]
+pub struct FlyshotIcspResult {
+    /// 路点绝对时间连续数组指针。
+    pub waypoint_times: *mut f64,
+    /// 路点时间数组长度。
+    pub waypoint_times_len: i32,
+    /// 分段时长连续数组指针。
+    pub segment_durations: *mut f64,
+    /// 分段时长数组长度。
+    pub segment_durations_len: i32,
+    /// 分段缩放连续数组指针。
+    pub segment_scales: *mut f64,
+    /// 分段缩放数组长度。
+    pub segment_scales_len: i32,
+    /// 历史最优结果对应的迭代轮数。
+    pub iterations: i32,
+    /// 历史最小收敛指标。
+    pub threshold: f64,
+}
+
+/// 通过稳定 C ABI 执行 ICSP 规划。
+///
+/// # Safety
+/// 调用方必须保证所有非空输入指针在调用期间指向满足声明长度的可读连续数组，
+/// `result` 必须指向可写的结果指针槽位；返回的结果只能交给
+/// `flyshot_icsp_result_free` 释放。
+#[no_mangle]
+pub unsafe extern "C" fn flyshot_icsp_plan(
+    waypoints: *const f64,
+    waypoint_count: i32,
+    dof: i32,
+    velocity_limits: *const f64,
+    acceleration_limits: *const f64,
+    jerk_limits: *const f64,
+    options: *const FlyshotIcspOptions,
+    result: *mut *mut FlyshotIcspResult,
+) -> i32 {
+    clear_last_error();
+
+    // Debug/测试构建可恢复遗漏 panic；Release panic=abort 下仍依赖边界校验避免 panic。
+    match catch_unwind(AssertUnwindSafe(|| unsafe {
+        flyshot_icsp_plan_impl(
+            waypoints,
+            waypoint_count,
+            dof,
+            velocity_limits,
+            acceleration_limits,
+            jerk_limits,
+            options,
+            result,
+        )
+    })) {
+        Ok(code) => code,
+        Err(_) => {
+            set_last_error("Rust ICSP FFI 捕获到未预期的内部 panic");
+            FLYSHOT_ERROR_INTERNAL
+        }
+    }
+}
+
+/// 释放由 `flyshot_icsp_plan` 返回的结果及其全部连续数组。
+///
+/// # Safety
+/// `result` 必须为空指针，或是尚未释放且由本库当前版本分配的结果指针。
+#[no_mangle]
+pub unsafe extern "C" fn flyshot_icsp_result_free(result: *mut FlyshotIcspResult) {
+    if result.is_null() {
+        return;
+    }
+
+    // 先收回结果结构所有权，再按原始长度重建 boxed slice 归还数组内存。
+    let result = unsafe { Box::from_raw(result) };
+    unsafe {
+        free_f64_array(result.waypoint_times, result.waypoint_times_len);
+        free_f64_array(result.segment_durations, result.segment_durations_len);
+        free_f64_array(result.segment_scales, result.segment_scales_len);
+    }
+}
+
+/// 返回当前线程最近一次原生错误的 UTF-8 C 字符串。
+///
+/// # 返回值
+/// 指针在当前线程下一次原生调用前有效；没有错误时指向空字符串。
+#[no_mangle]
+pub extern "C" fn flyshot_last_error_message() -> *const c_char {
+    LAST_ERROR_MESSAGE.with(|message| message.borrow().as_ptr())
+}
+
+/// 执行已通过 panic 边界保护的实际 FFI 参数校验和规划调用。
+///
+/// # Safety
+/// 参数约束与公开 `flyshot_icsp_plan` 相同，本方法会在解引用前检查空指针和维度。
+#[allow(clippy::too_many_arguments)]
+unsafe fn flyshot_icsp_plan_impl(
+    waypoints: *const f64,
+    waypoint_count: i32,
+    dof: i32,
+    velocity_limits: *const f64,
+    acceleration_limits: *const f64,
+    jerk_limits: *const f64,
+    options: *const FlyshotIcspOptions,
+    result: *mut *mut FlyshotIcspResult,
+) -> i32 {
+    if result.is_null() {
+        return fail(FLYSHOT_ERROR_INVALID_INPUT, "result 输出指针不能为空");
+    }
+    unsafe {
+        *result = ptr::null_mut();
+    }
+
+    if waypoints.is_null() {
+        return fail(FLYSHOT_ERROR_INVALID_INPUT, "waypoints 输入指针不能为空");
+    }
+    if velocity_limits.is_null() || acceleration_limits.is_null() || jerk_limits.is_null() {
+        return fail(
+            FLYSHOT_ERROR_INVALID_INPUT,
+            "velocity/acceleration/jerk 限值指针不能为空",
+        );
+    }
+    if options.is_null() {
+        return fail(FLYSHOT_ERROR_INVALID_INPUT, "options 输入指针不能为空");
+    }
+    if waypoint_count < 4 || dof <= 0 {
+        return fail(
+            FLYSHOT_ERROR_INVALID_INPUT,
+            "waypoint_count 必须至少为 4，dof 必须大于零",
+        );
+    }
+
+    let Ok(waypoint_count_usize) = usize::try_from(waypoint_count) else {
+        return fail(FLYSHOT_ERROR_INVALID_INPUT, "waypoint_count 转换失败");
+    };
+    let Ok(dof_usize) = usize::try_from(dof) else {
+        return fail(FLYSHOT_ERROR_INVALID_INPUT, "dof 转换失败");
+    };
+    let Some(waypoint_value_count) = waypoint_count_usize.checked_mul(dof_usize) else {
+        return fail(FLYSHOT_ERROR_INVALID_INPUT, "路点数组长度计算溢出");
+    };
+    let ffi_options = unsafe { *options };
+    if ffi_options.max_iterations < 0 {
+        return fail(FLYSHOT_ERROR_INVALID_INPUT, "max_iterations 不能为负数");
+    }
+
+    // 所有原始指针都已完成非空及长度上界校验，此处才建立只读切片。
+    let input = IcspInput {
+        waypoints: unsafe { slice::from_raw_parts(waypoints, waypoint_value_count) },
+        waypoint_count: waypoint_count_usize,
+        dof: dof_usize,
+        velocity_limits: unsafe { slice::from_raw_parts(velocity_limits, dof_usize) },
+        acceleration_limits: unsafe { slice::from_raw_parts(acceleration_limits, dof_usize) },
+        jerk_limits: unsafe { slice::from_raw_parts(jerk_limits, dof_usize) },
+        options: IcspOptions {
+            threshold: ffi_options.threshold,
+            max_iterations: ffi_options.max_iterations as usize,
+            enforce_final_scale: ffi_options.enforce_final_scale != 0,
+            final_scale_tolerance: ffi_options.final_scale_tolerance,
+        },
+    };
+
+    match plan_icsp(&input) {
+        Ok(plan) => {
+            let Ok(iterations) = i32::try_from(plan.iterations) else {
+                return fail(FLYSHOT_ERROR_INTERNAL, "迭代次数超出 C ABI i32 范围");
+            };
+            let (waypoint_times, waypoint_times_len) = leak_f64_array(plan.waypoint_times);
+            let (segment_durations, segment_durations_len) = leak_f64_array(plan.segment_durations);
+            let (segment_scales, segment_scales_len) = leak_f64_array(plan.segment_scales);
+            let native_result = Box::new(FlyshotIcspResult {
+                waypoint_times,
+                waypoint_times_len,
+                segment_durations,
+                segment_durations_len,
+                segment_scales,
+                segment_scales_len,
+                iterations,
+                threshold: plan.threshold,
+            });
+            unsafe {
+                *result = Box::into_raw(native_result);
+            }
+            FLYSHOT_SUCCESS
+        }
+        Err(error) => {
+            let code = error_code(&error);
+            set_last_error(&error.to_string());
+            code
+        }
+    }
+}
+
+/// 将 Rust 规划错误映射为稳定 C ABI 错误码。
+///
+/// # 参数
+/// - `error`: 纯算法层返回的错误。
+///
+/// # 返回值
+/// 与错误类别一一对应的固定整数。
+fn error_code(error: &NativePlanningError) -> i32 {
+    match error {
+        NativePlanningError::InvalidInput(_) => FLYSHOT_ERROR_INVALID_INPUT,
+        NativePlanningError::NumericalFailure(_) => FLYSHOT_ERROR_NUMERICAL_FAILURE,
+        NativePlanningError::NotConverged(_) => FLYSHOT_ERROR_NOT_CONVERGED,
+    }
+}
+
+/// 写入错误文本并返回指定错误码。
+///
+/// # 参数
+/// - `code`: 需要返回给调用方的稳定错误码。
+/// - `message`: 当前线程可读取的诊断文本。
+///
+/// # 返回值
+/// 原样返回 `code`，便于校验分支直接结束。
+fn fail(code: i32, message: &str) -> i32 {
+    set_last_error(message);
+    code
+}
+
+/// 清空当前线程的最近错误文本。
+fn clear_last_error() {
+    set_last_error("");
+}
+
+/// 将诊断文本保存到当前线程；内部 NUL 会替换为空格以保持合法 C 字符串。
+///
+/// # 参数
+/// - `message`: 需要保存到线程局部缓冲区的 UTF-8 文本。
+fn set_last_error(message: &str) {
+    let sanitized = message.replace('\0', " ");
+    // FFI 路径明确不使用 unwrap；即使替换逻辑未来变化，也保持无 panic 回退。
+    #[allow(clippy::manual_unwrap_or_default)]
+    let c_string = match CString::new(sanitized) {
+        Ok(value) => value,
+        Err(_) => CString::default(),
+    };
+    LAST_ERROR_MESSAGE.with(|slot| {
+        *slot.borrow_mut() = c_string;
+    });
+}
+
+/// 将 `Vec<f64>` 转为由 C ABI 结果持有的裸指针和 i32 长度。
+///
+/// # 参数
+/// - `values`: 需要转移给结果对象的连续数组。
+///
+/// # 返回值
+/// 数组裸指针及其长度；当前输入长度来源于 i32 路点维度，因此不会溢出。
+fn leak_f64_array(values: Vec<f64>) -> (*mut f64, i32) {
+    let mut values = values.into_boxed_slice();
+    let length = i32::try_from(values.len()).unwrap_or(i32::MAX);
+    let pointer = values.as_mut_ptr();
+    std::mem::forget(values);
+    (pointer, length)
+}
+
+/// 按原始长度重建并释放 Rust 分配的 `f64` boxed slice。
+///
+/// # Safety
+/// 指针和长度必须来自 `leak_f64_array` 且尚未释放。
+unsafe fn free_f64_array(pointer: *mut f64, length: i32) {
+    if pointer.is_null() || length <= 0 {
+        return;
+    }
+    let Ok(length) = usize::try_from(length) else {
+        return;
+    };
+    let slice_pointer = ptr::slice_from_raw_parts_mut(pointer, length);
+    drop(unsafe { Box::from_raw(slice_pointer) });
+}
+
+#[cfg(test)]
+mod tests {
+    use std::ffi::CStr;
+    use std::ptr;
+
+    use super::{
+        flyshot_icsp_plan, flyshot_icsp_result_free, flyshot_last_error_message,
+        FlyshotIcspOptions, FlyshotIcspResult, FLYSHOT_ERROR_INVALID_INPUT, FLYSHOT_SUCCESS,
+    };
+
+    /// 验证空输入指针会返回输入错误，并在线程局部错误文本中保留可诊断信息。
+    #[test]
+    fn ffi_rejects_null_waypoint_pointer_with_error_message() {
+        let options = FlyshotIcspOptions::default();
+        let mut result: *mut FlyshotIcspResult = ptr::null_mut();
+
+        let error_code = unsafe {
+            flyshot_icsp_plan(
+                ptr::null(),
+                4,
+                1,
+                [2.0].as_ptr(),
+                [4.0].as_ptr(),
+                [8.0].as_ptr(),
+                &options,
+                &mut result,
+            )
+        };
+
+        assert_eq!(error_code, FLYSHOT_ERROR_INVALID_INPUT);
+        assert!(result.is_null());
+        let message = unsafe { CStr::from_ptr(flyshot_last_error_message()) }
+            .to_string_lossy()
+            .into_owned();
+        assert!(message.contains("waypoints"));
+    }
+
+    /// 验证正常 C ABI 调用会返回 Rust 分配的结果，并可通过唯一释放函数归还。
+    #[test]
+    fn ffi_returns_and_frees_successful_plan_result() {
+        let waypoints = [0.0, 0.5, 1.0, 1.5];
+        let velocity_limits = [2.0];
+        let acceleration_limits = [4.0];
+        let jerk_limits = [8.0];
+        let options = FlyshotIcspOptions {
+            enforce_final_scale: 0,
+            ..FlyshotIcspOptions::default()
+        };
+        let mut result: *mut FlyshotIcspResult = ptr::null_mut();
+
+        let error_code = unsafe {
+            flyshot_icsp_plan(
+                waypoints.as_ptr(),
+                4,
+                1,
+                velocity_limits.as_ptr(),
+                acceleration_limits.as_ptr(),
+                jerk_limits.as_ptr(),
+                &options,
+                &mut result,
+            )
+        };
+
+        let message = unsafe { CStr::from_ptr(flyshot_last_error_message()) }
+            .to_string_lossy()
+            .into_owned();
+        assert_eq!(error_code, FLYSHOT_SUCCESS, "{message}");
+        assert!(!result.is_null());
+        unsafe {
+            assert_eq!((*result).waypoint_times_len, 4);
+            assert_eq!((*result).segment_durations_len, 3);
+            assert_eq!((*result).segment_scales_len, 3);
+            flyshot_icsp_result_free(result);
+        }
+    }
+
+    /// 验证包含非有限数的路点会在 FFI 内转换为输入错误而不是触发 panic。
+    #[test]
+    fn ffi_rejects_non_finite_waypoint_values() {
+        let waypoints = [0.0, 0.5, f64::NAN, 1.5];
+        let velocity_limits = [2.0];
+        let acceleration_limits = [4.0];
+        let jerk_limits = [8.0];
+        let options = FlyshotIcspOptions::default();
+        let mut result: *mut FlyshotIcspResult = ptr::null_mut();
+
+        let error_code = unsafe {
+            flyshot_icsp_plan(
+                waypoints.as_ptr(),
+                4,
+                1,
+                velocity_limits.as_ptr(),
+                acceleration_limits.as_ptr(),
+                jerk_limits.as_ptr(),
+                &options,
+                &mut result,
+            )
+        };
+
+        assert_eq!(error_code, FLYSHOT_ERROR_INVALID_INPUT);
+        assert!(result.is_null());
+        let message = unsafe { CStr::from_ptr(flyshot_last_error_message()) }
+            .to_string_lossy()
+            .into_owned();
+        assert!(message.contains("有限数"));
+    }
+}

native/flyshot-native-planning/src/icsp.rs

@@ -0,0 +1,367 @@
+//! 纯 Rust ICSP 规划算法。
+
+use crate::error::NativePlanningError;
+use crate::spline::CubicSpline1D;
+
+/// 描述 ICSP 迭代和最终约束检查选项。
+#[derive(Debug, Clone, Copy, PartialEq)]
+pub struct IcspOptions {
+    /// 收敛指标 `sum(abs(scale - 1))` 的停止阈值。
+    pub threshold: f64,
+    /// 最大迭代索引；算法与托管实现一致，会执行 `0..=max_iterations`。
+    pub max_iterations: usize,
+    /// 是否在历史最优结果仍有 scale 超限时返回失败。
+    pub enforce_final_scale: bool,
+    /// 最终 scale 检查允许的非负容差。
+    pub final_scale_tolerance: f64,
+}
+
+impl Default for IcspOptions {
+    /// 返回与现有 C# `ICspPlanner` 一致的默认参数。
+    ///
+    /// # 返回值
+    /// threshold=1e-3、max_iterations=1000、启用最终 scale 检查且容差为 1e-2。
+    fn default() -> Self {
+        Self {
+            threshold: 1e-3,
+            max_iterations: 1000,
+            enforce_final_scale: true,
+            final_scale_tolerance: 1e-2,
+        }
+    }
+}
+
+/// 描述一次纯 Rust ICSP 调用的扁平输入。
+#[derive(Debug, Clone, Copy)]
+pub struct IcspInput<'a> {
+    /// 按路点行优先排列的连续关节位置数组。
+    pub waypoints: &'a [f64],
+    /// 路点数量。
+    pub waypoint_count: usize,
+    /// 每个路点的关节自由度。
+    pub dof: usize,
+    /// 每个关节的正速度上限。
+    pub velocity_limits: &'a [f64],
+    /// 每个关节的正加速度上限。
+    pub acceleration_limits: &'a [f64],
+    /// 每个关节的正 jerk 上限。
+    pub jerk_limits: &'a [f64],
+    /// 本次规划使用的迭代选项。
+    pub options: IcspOptions,
+}
+
+/// 表示 ICSP 历史最优迭代对应的完整规划结果。
+#[derive(Debug, Clone, PartialEq)]
+pub struct IcspPlanResult {
+    /// 每个路点对应的绝对时间，首项固定为零。
+    pub waypoint_times: Vec<f64>,
+    /// 历史最优结果的逐段时长。
+    pub segment_durations: Vec<f64>,
+    /// 历史最优结果的逐段约束缩放因子。
+    pub segment_scales: Vec<f64>,
+    /// 历史最优结果产生时已执行的迭代轮数。
+    pub iterations: usize,
+    /// 历史最小 `sum(abs(scale - 1))`。
+    pub threshold: f64,
+}
+
+/// 执行 ICSP 迭代规划并返回历史最优结果。
+///
+/// # 参数
+/// - `input`: 行优先路点、逐关节约束和迭代选项。
+///
+/// # 返回值
+/// 输入合法并通过最终 scale 检查时返回规划结果；否则返回明确错误。
+pub fn plan_icsp(input: &IcspInput<'_>) -> Result<IcspPlanResult, NativePlanningError> {
+    validate_input(input)?;
+
+    let mut segment_durations = compute_initial_durations(input)?;
+    let segment_count = segment_durations.len();
+    let mut best_result: Option<IcspPlanResult> = None;
+    let mut best_threshold = f64::INFINITY;
+
+    // 迭代范围必须包含 max_iterations 对应轮次，以复刻托管实现的 `<=` 语义。
+    for iteration in 0..=input.options.max_iterations {
+        let waypoint_times = cumulative_times(&segment_durations)?;
+        let splines = build_joint_splines(input, &waypoint_times)?;
+        let mut segment_scales = vec![0.0; segment_count];
+
+        for (segment, segment_scale) in segment_scales.iter_mut().enumerate() {
+            let mut scale = 0.0_f64;
+            for (joint, spline) in splines.iter().enumerate() {
+                let [max_velocity, max_acceleration, max_jerk] =
+                    spline.segment_peak_derivatives(segment)?;
+                let velocity_scale = (max_velocity / input.velocity_limits[joint]).abs();
+                let acceleration_scale = (max_acceleration / input.acceleration_limits[joint])
+                    .abs()
+                    .sqrt();
+                let jerk_scale = (max_jerk / input.jerk_limits[joint]).abs().cbrt();
+                scale = scale.max(velocity_scale.max(acceleration_scale.max(jerk_scale)));
+            }
+            if !scale.is_finite() {
+                return Err(NativePlanningError::NumericalFailure(format!(
+                    "段 {segment} 的缩放因子不是有限数"
+                )));
+            }
+            *segment_scale = scale;
+        }
+
+        let current_threshold = segment_scales
+            .iter()
+            .map(|scale| (scale - 1.0).abs())
+            .sum::<f64>();
+        if !current_threshold.is_finite() {
+            return Err(NativePlanningError::NumericalFailure(
+                "ICSP 收敛指标不是有限数".to_owned(),
+            ));
+        }
+
+        // 始终保存历史最小指标对应的数据，不能只保留最后一轮。
+        if current_threshold < best_threshold {
+            best_threshold = current_threshold;
+            best_result = Some(IcspPlanResult {
+                waypoint_times: waypoint_times.clone(),
+                segment_durations: segment_durations.clone(),
+                segment_scales: segment_scales.clone(),
+                iterations: iteration + 1,
+                threshold: current_threshold,
+            });
+        }
+
+        if current_threshold < input.options.threshold {
+            break;
+        }
+
+        for (duration, scale) in segment_durations.iter_mut().zip(segment_scales.iter()) {
+            *duration *= *scale;
+        }
+    }
+
+    let Some(result) = best_result else {
+        return Err(NativePlanningError::NumericalFailure(
+            "ICSP 未产生任何有效迭代结果".to_owned(),
+        ));
+    };
+    let global_scale = result
+        .segment_scales
+        .iter()
+        .copied()
+        .fold(0.0_f64, f64::max);
+
+    if input.options.enforce_final_scale && global_scale > 1.0 + input.options.final_scale_tolerance
+    {
+        return Err(NativePlanningError::NotConverged(format!(
+            "global_scale={global_scale:.6} > {:.6}",
+            1.0 + input.options.final_scale_tolerance
+        )));
+    }
+
+    Ok(result)
+}
+
+/// 校验 ICSP 输入维度、有限性和正约束。
+///
+/// # 参数
+/// - `input`: 待校验的纯算法输入。
+///
+/// # 返回值
+/// 全部约束满足时返回 `Ok(())`，否则返回输入错误。
+fn validate_input(input: &IcspInput<'_>) -> Result<(), NativePlanningError> {
+    if input.waypoint_count < 4 {
+        return Err(NativePlanningError::InvalidInput(
+            "ICSP 至少需要 4 个示教点".to_owned(),
+        ));
+    }
+    if input.dof == 0 {
+        return Err(NativePlanningError::InvalidInput(
+            "自由度必须大于零".to_owned(),
+        ));
+    }
+    let expected_waypoint_values = input
+        .waypoint_count
+        .checked_mul(input.dof)
+        .ok_or_else(|| NativePlanningError::InvalidInput("路点维度乘积溢出".to_owned()))?;
+    if input.waypoints.len() != expected_waypoint_values {
+        return Err(NativePlanningError::InvalidInput(format!(
+            "路点扁平数组长度应为 {expected_waypoint_values}，实际为 {}",
+            input.waypoints.len()
+        )));
+    }
+    if input.velocity_limits.len() != input.dof
+        || input.acceleration_limits.len() != input.dof
+        || input.jerk_limits.len() != input.dof
+    {
+        return Err(NativePlanningError::InvalidInput(
+            "速度、加速度和 jerk 限值长度必须等于自由度".to_owned(),
+        ));
+    }
+    if input.waypoints.iter().any(|value| !value.is_finite()) {
+        return Err(NativePlanningError::InvalidInput(
+            "路点位置必须全部为有限数".to_owned(),
+        ));
+    }
+    if input
+        .velocity_limits
+        .iter()
+        .chain(input.acceleration_limits.iter())
+        .chain(input.jerk_limits.iter())
+        .any(|limit| !limit.is_finite() || *limit <= 0.0)
+    {
+        return Err(NativePlanningError::InvalidInput(
+            "速度、加速度和 jerk 限值必须全部为有限正数".to_owned(),
+        ));
+    }
+    if !input.options.threshold.is_finite() || input.options.threshold <= 0.0 {
+        return Err(NativePlanningError::InvalidInput(
+            "收敛阈值必须为有限正数".to_owned(),
+        ));
+    }
+    if !input.options.final_scale_tolerance.is_finite() || input.options.final_scale_tolerance < 0.0
+    {
+        return Err(NativePlanningError::InvalidInput(
+            "最终 scale 容差必须为有限非负数".to_owned(),
+        ));
+    }
+    Ok(())
+}
+
+/// 计算相邻路点在关节空间的欧氏距离，作为初始段时长。
+///
+/// # 参数
+/// - `input`: 已通过维度校验的 ICSP 输入。
+///
+/// # 返回值
+/// 每对相邻路点对应的正段时长。
+fn compute_initial_durations(input: &IcspInput<'_>) -> Result<Vec<f64>, NativePlanningError> {
+    let mut durations = Vec::with_capacity(input.waypoint_count - 1);
+    for segment in 0..input.waypoint_count - 1 {
+        let start_offset = segment * input.dof;
+        let end_offset = (segment + 1) * input.dof;
+        let mut squared_distance = 0.0;
+        for joint in 0..input.dof {
+            let difference =
+                input.waypoints[end_offset + joint] - input.waypoints[start_offset + joint];
+            squared_distance += difference * difference;
+        }
+        let duration = squared_distance.sqrt();
+        if !duration.is_finite() || duration <= 0.0 {
+            return Err(NativePlanningError::InvalidInput(format!(
+                "相邻路点段 {segment} 的欧氏距离必须为有限正数"
+            )));
+        }
+        durations.push(duration);
+    }
+    Ok(durations)
+}
+
+/// 将逐段时长累加为首项为零的绝对时间节点。
+///
+/// # 参数
+/// - `segment_durations`: 当前迭代的逐段时长。
+///
+/// # 返回值
+/// 长度比分段时长多一的严格递增时间轴。
+fn cumulative_times(segment_durations: &[f64]) -> Result<Vec<f64>, NativePlanningError> {
+    let mut times = Vec::with_capacity(segment_durations.len() + 1);
+    times.push(0.0);
+    let mut current = 0.0;
+    for (segment, duration) in segment_durations.iter().copied().enumerate() {
+        if !duration.is_finite() || duration <= 0.0 {
+            return Err(NativePlanningError::NumericalFailure(format!(
+                "段 {segment} 的迭代时长必须为有限正数"
+            )));
+        }
+        current += duration;
+        if !current.is_finite() {
+            return Err(NativePlanningError::NumericalFailure(
+                "累计时间轴发生溢出".to_owned(),
+            ));
+        }
+        times.push(current);
+    }
+    Ok(times)
+}
+
+/// 为每个关节独立构造 clamped-zero 样条。
+///
+/// # 参数
+/// - `input`: 行优先路点及自由度信息。
+/// - `waypoint_times`: 当前迭代的绝对时间节点。
+///
+/// # 返回值
+/// 按关节顺序排列的单自由度样条集合。
+fn build_joint_splines(
+    input: &IcspInput<'_>,
+    waypoint_times: &[f64],
+) -> Result<Vec<CubicSpline1D>, NativePlanningError> {
+    let mut splines = Vec::with_capacity(input.dof);
+    for joint in 0..input.dof {
+        let mut values = Vec::with_capacity(input.waypoint_count);
+        for waypoint in 0..input.waypoint_count {
+            values.push(input.waypoints[waypoint * input.dof + joint]);
+        }
+        splines.push(CubicSpline1D::new_clamped_zero(waypoint_times, &values)?);
+    }
+    Ok(splines)
+}
+
+#[cfg(test)]
+mod tests {
+    use super::{plan_icsp, IcspInput, IcspOptions};
+    use crate::error::NativePlanningError;
+
+    /// 验证 ICSP 对基础四路点样本返回完整时间轴、分段结果和有限收敛信息。
+    #[test]
+    fn icsp_returns_complete_result_for_basic_fixture() {
+        let waypoints = [0.0, 0.0, 1.0, 0.5, 2.0, 1.0, 3.0, 1.5];
+        let velocity_limits = [2.0, 2.0];
+        let acceleration_limits = [4.0, 4.0];
+        let jerk_limits = [8.0, 8.0];
+        let input = IcspInput {
+            waypoints: &waypoints,
+            waypoint_count: 4,
+            dof: 2,
+            velocity_limits: &velocity_limits,
+            acceleration_limits: &acceleration_limits,
+            jerk_limits: &jerk_limits,
+            options: IcspOptions::default(),
+        };
+
+        let result = plan_icsp(&input).unwrap();
+
+        assert_eq!(result.waypoint_times.len(), 4);
+        assert_eq!(result.segment_durations.len(), 3);
+        assert_eq!(result.segment_scales.len(), 3);
+        assert!(result.iterations >= 1);
+        assert!(result.threshold.is_finite());
+        assert!(result
+            .waypoint_times
+            .windows(2)
+            .all(|pair| pair[1] > pair[0]));
+    }
+
+    /// 验证禁用迭代且最终 scale 明显超限时，纯 Rust 核心返回未收敛错误。
+    #[test]
+    fn icsp_reports_not_converged_when_zero_iterations_cannot_meet_limits() {
+        let waypoints = [0.0, 1.0, 2.0, 3.0];
+        let velocity_limits = [0.01];
+        let acceleration_limits = [0.01];
+        let jerk_limits = [0.01];
+        let input = IcspInput {
+            waypoints: &waypoints,
+            waypoint_count: 4,
+            dof: 1,
+            velocity_limits: &velocity_limits,
+            acceleration_limits: &acceleration_limits,
+            jerk_limits: &jerk_limits,
+            options: IcspOptions {
+                max_iterations: 0,
+                ..IcspOptions::default()
+            },
+        };
+
+        let error = plan_icsp(&input).unwrap_err();
+
+        assert!(matches!(error, NativePlanningError::NotConverged(_)));
+    }
+}

native/flyshot-native-planning/src/lib.rs

@@ -0,0 +1,6 @@
+//! Flyshot ICSP 原生规划库的稳定入口。
+
+mod error;
+mod ffi;
+mod icsp;
+mod spline;

native/flyshot-native-planning/src/spline.rs

@@ -0,0 +1,277 @@
+//! clamped-zero 三次样条算法。
+
+use crate::error::NativePlanningError;
+
+/// 表示单自由度 clamped-zero 三次样条及其逐段多项式系数。
+#[derive(Debug, Clone)]
+pub struct CubicSpline1D {
+    knots: Vec<f64>,
+    coefficients: Vec<[f64; 4]>,
+}
+
+// 位置和逐阶导数求值是第一阶段要求的原生能力，当前 ICSP 热路径只直接使用解析峰值。
+#[allow(dead_code)]
+impl CubicSpline1D {
+    /// 使用严格递增时间节点和对应值构造首尾速度为零的三次样条。
+    ///
+    /// # 参数
+    /// - `times`: 严格递增且全部有限的时间节点，长度至少为 2。
+    /// - `values`: 与时间节点一一对应的有限位置值。
+    ///
+    /// # 返回值
+    /// 输入合法且三对角方程可解时返回样条，否则返回输入或数值错误。
+    pub fn new_clamped_zero(times: &[f64], values: &[f64]) -> Result<Self, NativePlanningError> {
+        if times.len() < 2 {
+            return Err(NativePlanningError::InvalidInput(
+                "时间节点至少需要 2 个".to_owned(),
+            ));
+        }
+        if values.len() != times.len() {
+            return Err(NativePlanningError::InvalidInput(
+                "位置值数量必须与时间节点数量一致".to_owned(),
+            ));
+        }
+        if times.iter().any(|value| !value.is_finite())
+            || values.iter().any(|value| !value.is_finite())
+        {
+            return Err(NativePlanningError::InvalidInput(
+                "时间节点和位置值必须全部为有限数".to_owned(),
+            ));
+        }
+
+        let segment_count = times.len() - 1;
+        let mut durations = Vec::with_capacity(segment_count);
+        for segment in 0..segment_count {
+            let duration = times[segment + 1] - times[segment];
+            if duration <= 0.0 || !duration.is_finite() {
+                return Err(NativePlanningError::InvalidInput(format!(
+                    "时间节点必须严格递增，段 {segment} 的时长为 {duration}"
+                )));
+            }
+            durations.push(duration);
+        }
+
+        // 与托管实现保持一致：先组装 clamped-zero 边界下的三对角方程右端项。
+        let node_count = times.len();
+        let mut alpha = vec![0.0; node_count];
+        alpha[0] = 6.0 * ((values[1] - values[0]) / durations[0]);
+        alpha[node_count - 1] = 6.0
+            * (0.0 - (values[node_count - 1] - values[node_count - 2]) / durations[node_count - 2]);
+        for index in 1..node_count - 1 {
+            alpha[index] = 6.0
+                * ((values[index + 1] - values[index]) / durations[index]
+                    - (values[index] - values[index - 1]) / durations[index - 1]);
+        }
+
+        let mut diagonal = vec![0.0; node_count];
+        let mut upper_ratio = vec![0.0; node_count];
+        let mut solved_rhs = vec![0.0; node_count];
+        diagonal[0] = 2.0 * durations[0];
+        upper_ratio[0] = 0.5;
+        solved_rhs[0] = alpha[0] / diagonal[0];
+
+        // Thomas 消元逐行处理内部节点，避免建立完整稠密矩阵。
+        for index in 1..node_count - 1 {
+            diagonal[index] = 2.0 * (durations[index - 1] + durations[index])
+                - durations[index - 1] * upper_ratio[index - 1];
+            if diagonal[index].abs() <= f64::EPSILON || !diagonal[index].is_finite() {
+                return Err(NativePlanningError::NumericalFailure(format!(
+                    "三对角方程在节点 {index} 出现无效主元"
+                )));
+            }
+            upper_ratio[index] = durations[index] / diagonal[index];
+            solved_rhs[index] =
+                (alpha[index] - durations[index - 1] * solved_rhs[index - 1]) / diagonal[index];
+        }
+
+        diagonal[node_count - 1] = 2.0 * durations[node_count - 2]
+            - durations[node_count - 2] * upper_ratio[node_count - 2];
+        if diagonal[node_count - 1].abs() <= f64::EPSILON || !diagonal[node_count - 1].is_finite() {
+            return Err(NativePlanningError::NumericalFailure(
+                "三对角方程末端出现无效主元".to_owned(),
+            ));
+        }
+        solved_rhs[node_count - 1] = (alpha[node_count - 1]
+            - durations[node_count - 2] * solved_rhs[node_count - 2])
+            / diagonal[node_count - 1];
+
+        // 回代得到各节点二阶导数，再转换为每段局部三次多项式系数。
+        let mut second_derivatives = vec![0.0; node_count];
+        second_derivatives[node_count - 1] = solved_rhs[node_count - 1];
+        for index in (0..node_count - 1).rev() {
+            second_derivatives[index] =
+                solved_rhs[index] - upper_ratio[index] * second_derivatives[index + 1];
+        }
+
+        let mut coefficients = Vec::with_capacity(segment_count);
+        for segment in 0..segment_count {
+            let duration = durations[segment];
+            let cubic =
+                (second_derivatives[segment + 1] - second_derivatives[segment]) / (6.0 * duration);
+            let quadratic = second_derivatives[segment] / 2.0;
+            let linear = (values[segment + 1] - values[segment]) / duration
+                - duration * (2.0 * second_derivatives[segment] + second_derivatives[segment + 1])
+                    / 6.0;
+            coefficients.push([cubic, quadratic, linear, values[segment]]);
+        }
+
+        Ok(Self {
+            knots: times.to_vec(),
+            coefficients,
+        })
+    }
+
+    /// 在指定绝对时间求样条位置；超出范围时使用首段或末段外推点。
+    ///
+    /// # 参数
+    /// - `time`: 需要求值的绝对时间。
+    ///
+    /// # 返回值
+    /// 对应样条位置。
+    pub fn position(&self, time: f64) -> f64 {
+        let (segment, local_time) = self.segment_and_local_time(time);
+        let [cubic, quadratic, linear, constant] = self.coefficients[segment];
+        cubic * local_time * local_time * local_time
+            + quadratic * local_time * local_time
+            + linear * local_time
+            + constant
+    }
+
+    /// 在指定绝对时间求一阶导数。
+    ///
+    /// # 参数
+    /// - `time`: 需要求值的绝对时间。
+    ///
+    /// # 返回值
+    /// 对应样条速度。
+    pub fn velocity(&self, time: f64) -> f64 {
+        let (segment, local_time) = self.segment_and_local_time(time);
+        let [cubic, quadratic, linear, _] = self.coefficients[segment];
+        3.0 * cubic * local_time * local_time + 2.0 * quadratic * local_time + linear
+    }
+
+    /// 在指定绝对时间求二阶导数。
+    ///
+    /// # 参数
+    /// - `time`: 需要求值的绝对时间。
+    ///
+    /// # 返回值
+    /// 对应样条加速度。
+    pub fn acceleration(&self, time: f64) -> f64 {
+        let (segment, local_time) = self.segment_and_local_time(time);
+        let [cubic, quadratic, _, _] = self.coefficients[segment];
+        6.0 * cubic * local_time + 2.0 * quadratic
+    }
+
+    /// 在指定绝对时间求三阶导数。
+    ///
+    /// # 参数
+    /// - `time`: 需要求值的绝对时间。
+    ///
+    /// # 返回值
+    /// 对应样条 jerk。
+    pub fn jerk(&self, time: f64) -> f64 {
+        let (segment, _) = self.segment_and_local_time(time);
+        6.0 * self.coefficients[segment][0]
+    }
+
+    /// 解析计算指定段的一至三阶导数绝对值峰值。
+    ///
+    /// # 参数
+    /// - `segment_index`: 从零开始的段索引。
+    ///
+    /// # 返回值
+    /// `[速度峰值, 加速度峰值, jerk 峰值]`；索引越界时返回输入错误。
+    pub fn segment_peak_derivatives(
+        &self,
+        segment_index: usize,
+    ) -> Result<[f64; 3], NativePlanningError> {
+        let Some([cubic, quadratic, linear, _]) = self.coefficients.get(segment_index).copied()
+        else {
+            return Err(NativePlanningError::InvalidInput(format!(
+                "样条段索引 {segment_index} 越界"
+            )));
+        };
+        let duration = self.knots[segment_index + 1] - self.knots[segment_index];
+
+        // 一阶导为二次函数，绝对值峰值只可能出现在端点或段内顶点。
+        let mut velocity_peak = linear
+            .abs()
+            .max((3.0 * cubic * duration * duration + 2.0 * quadratic * duration + linear).abs());
+        if cubic.abs() > 1e-12 {
+            let critical = -quadratic / (3.0 * cubic);
+            if critical > 0.0 && critical < duration {
+                velocity_peak = velocity_peak.max(
+                    (3.0 * cubic * critical * critical + 2.0 * quadratic * critical + linear).abs(),
+                );
+            }
+        }
+
+        let acceleration_peak = (2.0 * quadratic)
+            .abs()
+            .max((6.0 * cubic * duration + 2.0 * quadratic).abs());
+        let jerk_peak = (6.0 * cubic).abs();
+        Ok([velocity_peak, acceleration_peak, jerk_peak])
+    }
+
+    /// 定位时间所在样条段并计算相对段起点的局部时间。
+    ///
+    /// # 参数
+    /// - `time`: 需要定位的绝对时间。
+    ///
+    /// # 返回值
+    /// `(段索引, 局部时间)`；时间超界时钳制到首尾节点。
+    fn segment_and_local_time(&self, time: f64) -> (usize, f64) {
+        let last_segment = self.coefficients.len() - 1;
+        if time <= self.knots[0] {
+            return (0, 0.0);
+        }
+
+        let last_knot = self.knots[self.knots.len() - 1];
+        if time >= last_knot {
+            return (
+                last_segment,
+                self.knots[last_segment + 1] - self.knots[last_segment],
+            );
+        }
+
+        let insertion = self.knots.partition_point(|knot| *knot <= time);
+        let segment = insertion.saturating_sub(1).min(last_segment);
+        (segment, time - self.knots[segment])
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::CubicSpline1D;
+
+    /// 验证 clamped-zero 样条严格命中首尾路点，并满足两端零速度边界。
+    #[test]
+    fn clamped_zero_spline_hits_waypoints_and_zero_boundary_velocity() {
+        let times = [0.0, 1.0, 2.0, 3.0];
+        let values = [0.0, 1.0, 0.5, 2.0];
+        let spline = CubicSpline1D::new_clamped_zero(&times, &values).unwrap();
+
+        assert!((spline.position(0.0) - 0.0).abs() <= 1e-12);
+        assert!((spline.position(3.0) - 2.0).abs() <= 1e-12);
+        assert!(spline.velocity(0.0).abs() <= 1e-12);
+        assert!(spline.velocity(3.0).abs() <= 1e-12);
+        assert!(spline.acceleration(1.5).is_finite());
+        assert!(spline.jerk(1.5).is_finite());
+    }
+
+    /// 验证每段速度、加速度和 jerk 的解析峰值不会漏掉段内速度极值。
+    #[test]
+    fn segment_peak_derivatives_include_internal_velocity_extremum() {
+        let times = [0.0, 1.0, 2.0, 3.0];
+        let values = [0.0, 1.0, 0.5, 2.0];
+        let spline = CubicSpline1D::new_clamped_zero(&times, &values).unwrap();
+
+        let peaks = spline.segment_peak_derivatives(0).unwrap();
+
+        assert!(peaks[0] >= spline.velocity(0.0).abs());
+        assert!(peaks[0] >= spline.velocity(1.0).abs());
+        assert!(peaks[1].is_finite() && peaks[1] > 0.0);
+        assert!(peaks[2].is_finite() && peaks[2] > 0.0);
+    }
+}