圆锥离散化

效果图：

matlab代码：

dot_AG = [3; 2; 5];
S_AR = [4; 2; 7];
n_radial = 3;
m_angular = 20;

vectors = discretized_cone_vectors(dot_AG, S_AR, n_radial, m_angular);
visualize_vectors(dot_AG, vectors);
function vectors = discretized_cone_vectors(dot_AG, S_AR, n_radial, m_angular)
    % 输入:
    % dot_AG: 圆锥顶点
    % S_AR: 圆锥轴向量
    % n_radial: 径向的离散化数量
    % m_angular: 周向的离散化数量

    % 输出:
    % vectors: 离散化的圆锥向量

    % 计算单位轴向量
    unit_axis = S_AR / norm(S_AR);

    % 初始化输出向量
    vectors = zeros(3, n_radial * m_angular);

    % 计算径向和周向的角度增量
    delta_radial = 30 / n_radial;  %%%%%%%%%%%%%%%%%%5
    delta_angular = 2 * pi / m_angular;

    % 为了围绕轴旋转，我们需要一个与轴正交的向量。这可以通过叉乘得到。
    % 这里我们简单地使用 [1, 0, 0] 叉乘轴向量。如果轴向量恰好是 [1, 0, 0]，我们可以使用 [0, 1, 0]。
    if isequal(unit_axis, [1; 0; 0])
        orthogonal_vector = cross(unit_axis, [0; 1; 0]);
    else
        orthogonal_vector = cross(unit_axis, [1; 0; 0]);
    end
    orthogonal_vector = orthogonal_vector / norm(orthogonal_vector);

    index = 1;
    for i = 1:n_radial
        for j = 1:m_angular
            % 计算旋转矩阵
            R_radial = RodriguesRotation(unit_axis, (i - 0.5) * delta_radial);
            R_angular = RodriguesRotation(unit_axis, j * delta_angular);

            % 计算离散化的向量
            rotated_vector = R_angular * R_radial * orthogonal_vector;

            vectors(:, index) = dot_AG + rotated_vector;
            index = index + 1;
        end
    end
end

function R = RodriguesRotation(axis, theta)
    % Rodrigues 旋转公式
    K = [0, -axis(3), axis(2); axis(3), 0, -axis(1); -axis(2), axis(1), 0];
    R = eye(3) + sin(theta) * K + (1 - cos(theta)) * K^2;
end
function visualize_vectors(dot_AG, vectors)
    % 输入:
    % dot_AG: 圆锥顶点
    % vectors: 离散化的圆锥向量

    figure;
    hold on;

    % 对于每一个向量进行可视化
    for i = 1:size(vectors, 2)
        % 使用线段连接dot_AG和向量
        plot3([dot_AG(1),5* vectors(1, i)], [dot_AG(2),5* vectors(2, i)], [dot_AG(3), 5*vectors(3, i)], 'b-');
    end

    xlabel('X');
    ylabel('Y');
    zlabel('Z');
    grid on;
    axis equal;
    title('Discretized Cone Vectors');
    view(3); % 设置三维视图
    hold off;
end