function constrained_lspia_b_spline()
clc;clear; close all
theta = linspace(0, pi, 1000)';
X = [cos(theta), sin(theta)];
X = [X; [cos(-theta), sin(-theta)]-[2, 0]] ;
n = length(X);
m = 50;
k = 3;
max_iter = 1000;
start_point = X(1, :);
end_point = X(end, :);
start_tangent = X(2, :) - X(1, :);
end_tangent = X(end, :) - X(end - 1, :);
start_tangent = start_tangent / norm(start_tangent);
end_tangent = end_tangent / norm(end_tangent);
chords = vecnorm(diff(X), 2, 2);
u = [0; cumsum(chords)/sum(chords)];
P = initialize_control_points(X, m, start_point, end_point, start_tangent, end_tangent);
p_1 = P(1:2,:);
p_n = P(end-1:end,:);
knots = constrained_knot_vector(u, k, m);
N = zeros(n, m);
for i = 1:n
span = findSpan(u(i), knots, m);
basis = basisFunctions(u(i), span, knots, k);
N(i, span-k:span) = basis;
end
lambda = 1.0;
for iter = 1:max_iter
Q = N * P;
R = X - Q;
deltaP = (N' * R) * lambda / n;
P(1:2,:) = p_1;
P(end-1:end,:) = p_n;
if deltaP < 1e-3
break;
end
end
verify_endpoint_constraints(N, P, start_point, end_point, start_tangent, end_tangent, knots, k);
u_eval = linspace(0, 1, 1000);
curve = zeros(length(u_eval), 2);
for i = 1:length(u_eval)
span = findSpan(u_eval(i), knots, m);
basis = basisFunctions(u_eval(i), span, knots, k);
curve(i,:) = basis * P(span-k:span, :);
end
figure;
plot(X(:,1), X(:,2), '.-', 'MarkerSize', 4, 'Color', [0.7 0.7 0.7]);
hold on;
plot(P(:,1), P(:,2), 'ro-', 'LineWidth', 1.5);
plot(curve(:,1), curve(:,2), 'b-', 'LineWidth', 2);
quiver(start_point(1), start_point(2), start_tangent(1)*0.2, start_tangent(2)*0.2, ...
'LineWidth', 2, 'Color', 'g', 'MaxHeadSize', 0.5);
quiver(end_point(1), end_point(2), end_tangent(1)*0.2, end_tangent(2)*0.2, ...
'LineWidth', 2, 'Color', 'm', 'MaxHeadSize', 0.5);
legend('原始数据', '控制点', '拟合曲线', '起点切向量', '终点切向量');
title(sprintf('带约束的LSPIA B样条拟合 (%d次迭代)', iter));
axis equal;
grid on;
end
function P = initialize_control_points(X, m, start_point, end_point, start_tangent, end_tangent)
idx = round(linspace(1, size(X,1), m));
P = X(idx, :);
P(1, :) = start_point;
P(end, :) = end_point;
P(2, :) = start_point + start_tangent * 0.1;
P(end-1, :) = end_point - end_tangent * 0.1;
end
function knots = constrained_knot_vector(u, k, m)
knots = linspace(0, 1, m - k + 1);
knots = [zeros(1, k), knots, ones(1, k)];
end
function verify_endpoint_constraints(N, P, start_point, end_point, start_tangent, end_tangent, knots, k)
fitted_start = N(1, :) * P;
position_error_start = norm(fitted_start - start_point);
fprintf('起点位置误差: %.2e\n', position_error_start);
fitted_end = N(end, :) * P;
position_error_end = norm(fitted_end - end_point);
fprintf('终点位置误差: %.2e\n', position_error_end);
u = 0.0001;
span0 = findSpan(0, knots, size(P,1));
basis0 = basisFunctions(0, span0, knots, k);
point0 = basis0 * P(span0-k:span0, :);
span1 = findSpan(u, knots, size(P,1));
basis1 = basisFunctions(u, span1, knots, k);
point1 = basis1 * P(span1-k:span1, :);
fitted_tangent = (point1 - point0) / u;
fitted_tangent = fitted_tangent / norm(fitted_tangent);
tangent_error_start = acos(dot(fitted_tangent, start_tangent/norm(start_tangent)));
fprintf('起点切向量角度误差: %.2f 度\n', rad2deg(tangent_error_start));
u_end = 1 - u;
span_end0 = findSpan(1, knots, size(P,1));
basis_end0 = basisFunctions(1, span_end0, knots, k);
point_end0 = basis_end0 * P(span_end0-k:span_end0, :);
span_end1 = findSpan(u_end, knots, size(P,1));
basis_end1 = basisFunctions(u_end, span_end1, knots, k);
point_end1 = basis_end1 * P(span_end1-k:span_end1, :);
fitted_tangent_end = (point_end0 - point_end1) / u;
fitted_tangent_end = fitted_tangent_end / norm(fitted_tangent_end);
tangent_error_end = acos(dot(fitted_tangent_end, end_tangent/norm(end_tangent)));
fprintf('终点切向量角度误差: %.2f 度\n', rad2deg(tangent_error_end));
end
function span = findSpan(u, knots, n_ctrl)
if u >= knots(n_ctrl+1)
span = n_ctrl;
return;
end
low = 1;
high = n_ctrl + 1;
mid = floor((low+high)/2);
while (u < knots(mid)) || (u >= knots(mid+1))
if u < knots(mid)
high = mid;
else
low = mid;
end
mid = floor((low+high)/2);
end
span = mid;
end
function N = basisFunctions(u, span, knots, k)
N = zeros(1, k+1);
left = zeros(1, k+1);
right = zeros(1, k+1);
N(1) = 1.0;
for j = 1:k
left(j+1) = u - knots(span+1-j);
right(j+1) = knots(span+j) - u;
saved = 0.0;
for r = 0:j-1
temp = N(r+1)/(right(r+2)+left(j-r+1));
N(r+1) = saved + right(r+2)*temp;
saved = left(j-r+1)*temp;
end
N(j+1) = saved;
end
end
转成c语言代码
最新发布
本文介绍了acos函数的基本概念，acos用于计算反余弦值。通过acos(-1.0)的例子，展示了如何求-1.0的反余弦，并将其赋值给double类型的常变量pi。
扫一扫
1518
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
