不用matlab内部函数的曲率采样法代码

以下是不用 MATLAB 内部函数的曲率采样法代码：

function [x, y] = curvature_sampling(xs, ys, ds)
    % xs, ys 为曲线上的点坐标，ds 为采样间隔
    x = xs(1);
    y = ys(1);
    k_old = 0;
    s = 0;
    for i = 2:length(xs)-1
        % 计算曲率
        k1 = ((xs(i+1)-xs(i))/ds - (xs(i)-xs(i-1))/ds) / ...
             ((xs(i+1)-xs(i))/ds^2 + (xs(i)-xs(i-1))/ds^2);
        k2 = ((ys(i+1)-ys(i))/ds - (ys(i)-ys(i-1))/ds) / ...
             ((ys(i+1)-ys(i))/ds^2 + (ys(i)-ys(i-1))/ds^2);
        k = (k1^2 + k2^2)^0.5;
        % 计算线段长度
        len = ((xs(i+1)-xs(i))^2 + (ys(i+1)-ys(i))^2)^0.5;
        s = s + len;
        % 若曲率变化过大则进行采样
        if abs(k - k_old) > 0.05 / ds
            x(end+1) = xs(i);
            y(end+1) = ys(i);
            k_old = k;
        end
    end
    % 最后一个点作为终点
    x(end+1) = xs(end);
    y(end+1) = ys(end);
end