MATLAB——计算立方体的法向量

深藏阁楼爱情的钟 2023-10-10 03:45 35阅读 0赞

### 文章目录 ###

*  绘制立方体
 *  计算面的法向量
 *  判断点是否在立方体内
 *   *  方法1：三角化后判断
     *  方法2：计算点与边界距离
 *  代码整合
 *  结果展示

# 绘制立方体 #

绘制立方体主要使用matlab自带的patch()函数，该函数具体用法参照博客[MATLAB——patch绘制多边形][MATLAB_patch]。

# 计算面的法向量 #

function [pos,n] = getNormalVector(cuboid)
    % cuboid: the handle returned by function path()
    % n: normal vectors of all flats
    
        vertices = cuboid.Vertices;
        faces = cuboid.Faces;
        n = zeros(size(faces,1),3);
        pos = zeros(size(faces,1),3);
    
        for i = 1:size(faces,1)
            index = faces(i,1:4);
            p1 = vertices(index(1),:)-vertices(index(2),:);
            p2 = vertices(index(2),:)-vertices(index(3),:);
    
            n(i,:) = cross(p1,p2);
            n(i,:) = -n(i,:)/norm(n(i,:));
            pos(i,:) = sum(vertices(index(:),:))/4;
            quiver3(pos(i,1),pos(i,2),pos(i,3),n(i,1),n(i,2),n(i,3),0.5);
        end
    
    end

# 判断点是否在立方体内 #

## 方法1：三角化后判断 ##

function inflag = inpolyhedron(point_set,p_detected)
    % point_set: a set of points stores the coordinates
    % p_detected: point to be detected
    % inflag:
    %     flag = 1: the point is in the polyhedron.
    %     flag = 0: the point is not in the polyhedron.
    
    % stores the coordinates of the convexes.
        tri = delaunayTriangulation(point_set);
        % number of the tetrahedrons decomposed from the polyhedron
        num_tet = size(tri,1);
        t_inflag = zeros(1,11);
        for i = 1:num_tet
            v1_coord = point_set(tri(i,1),:);
            v2_coord = point_set(tri(i,2),:);
            v3_coord = point_set(tri(i,3),:);
            v4_coord = point_set(tri(i,4),:);
            D0 =det( [v1_coord,1;v2_coord,1;v3_coord,1;v4_coord,1]);
            D1 = det([p_detected,1;v2_coord,1;v3_coord,1;v4_coord,1]);
            D2 = det([v1_coord,1;p_detected,1;v3_coord,1;v4_coord,1]);
            D3 = det([v1_coord,1;v2_coord,1;p_detected,1;v4_coord,1]);
            D4 = det([v1_coord,1;v2_coord,1;v3_coord,1;p_detected,1]);
    
            if D0*D1 > 0 && D0*D2>0 && D0*D3>0 && D0*D4 > 0
                t_inflag(i) = 1;
                break;
            end
    
        end
      
        if sum(t_inflag) > 0
            inflag = 1;
            disp('The point is in the polyhedron.');
        else
            inflag = 0;
            disp('The point is not in the polyhedron.');
        end
    end

## 方法2：计算点与边界距离 ##

function [minDist, dist]= distToArena(cuboid,normal,point)
        % Calculate the distance from a point outside the cuboid to the center of the cuboid.
        % Input:
        %   cuboid: the handle of cuboid returned by the function patch().
        %   normal: the normal of faces.
        %   point:  the point outside the cuboid.
        % return
        %   minDist: The distance between the point and the surface of the cuboid,
        %           the direction along the line connecting the point and the center of the cuboid.
        %   dist:    The distance between the center of the cuboid and the surface of the cuboid,
        %           the direction along the line connecting the point and the center of the cuboid.
        faces = cuboid.Faces;
        minDist = inf;
    
        arenaCenter = sum(cuboid.Vertices(:,:))/size(cuboid.Vertices,1);
        for i = 1:size(faces,1)
            angle = getAngle(normal(i,:),point-arenaCenter);
            if angle<=90
                for j=1:4
                    faceCenter = sum(cuboid.Vertices(faces(i,:),:))/4;
                end
                distToFace = norm((faceCenter-arenaCenter))/cos(deg2rad(angle));
                dir = point-arenaCenter;
                dir = dir/norm(dir);
                onFace = dir*distToFace+arenaCenter;
                
                vertices = cuboid.Vertices(faces(i,:),:);
                x = [min(vertices(:,1)), max(vertices(:,1))];
                y = [min(vertices(:,2)), max(vertices(:,2))];
                z = [min(vertices(:,3)), max(vertices(:,3))];
                
                if onFace(1)>=x(1) && onFace(1)<=x(2) ...
                   && onFace(2)>=y(1) && onFace(2)<=y(2) ...
                   && onFace(3)>=z(1) && onFace(3)<=z(2)
                    minDist = norm(point-arenaCenter) - distToFace;
                    dist = distToFace;
                    return
                end
            end
        end
    end

# 代码整合 #

cuboid = drawCuboid([1;2;2],[1;2;8],[0;0;0],'b',0.02);
    view([50,50,100]);
    point = [1.5,3,7];
    inpolyhedron(cuboid.Vertices,point)
    view(3)
    hold on
    [pos, normal] = getNormalVector(cuboid);
    
    [minDist, d] = distToArena(cuboid,normal,point);
    arenaControl(pos, normal, point)
    
    dir = (point - [1 2 8]);
    dir = dir/norm(dir);
    toFace = dir.*d+[1 2 8];
    plot3(toFace(1),toFace(2),toFace(3),'*')
    %quiver3(1,2,8,toFace(1),toFace(2),toFace(3));
    toPoint = dir.*(minDist+d)+[1 2 8];
    plot3(toPoint(1),toPoint(2),toPoint(3),'o')
    %quiver3(1,2,8,toPoint(1),toPoint(2),toPoint(3));
    plot3(point(1),point(2),point(3),'x')
    
    function inflag = inpolyhedron(point_set,p_detected)
    % point_set: a set of points stores the coordinates
    % p_detected: point to be detected
    % inflag:
    %     flag = 1: the point is in the polyhedron.
    %     flag = 0: the point is not in the polyhedron.
    
    % stores the coordinates of the convexes.
        tri = delaunayTriangulation(point_set);
        % number of the tetrahedrons decomposed from the polyhedron
        num_tet = size(tri,1);
        t_inflag = zeros(1,11);
        for i = 1:num_tet
            v1_coord = point_set(tri(i,1),:);
            v2_coord = point_set(tri(i,2),:);
            v3_coord = point_set(tri(i,3),:);
            v4_coord = point_set(tri(i,4),:);
            D0 =det( [v1_coord,1;v2_coord,1;v3_coord,1;v4_coord,1]);
            D1 = det([p_detected,1;v2_coord,1;v3_coord,1;v4_coord,1]);
            D2 = det([v1_coord,1;p_detected,1;v3_coord,1;v4_coord,1]);
            D3 = det([v1_coord,1;v2_coord,1;p_detected,1;v4_coord,1]);
            D4 = det([v1_coord,1;v2_coord,1;v3_coord,1;p_detected,1]);
    
            if D0*D1 > 0 && D0*D2>0 && D0*D3>0 && D0*D4 > 0
                t_inflag(i) = 1;
                break;
            end
    
        end
      
        if sum(t_inflag) > 0
            inflag = 1;
            disp('The point is in the polyhedron.');
        else
            inflag = 0;
            disp('The point is not in the polyhedron.');
        end
    end
    
    function [pos,n] = getNormalVector(cuboid)
    % cuboid: the handle returned by function path()
    % n: normal vectors of all flats
    
        vertices = cuboid.Vertices;
        faces = cuboid.Faces;
        n = zeros(size(faces,1),3);
        pos = zeros(size(faces,1),3);
    
        for i = 1:size(faces,1)
            index = faces(i,1:4);
            p1 = vertices(index(1),:)-vertices(index(2),:);
            p2 = vertices(index(2),:)-vertices(index(3),:);
    
            n(i,:) = cross(p1,p2);
            n(i,:) = -n(i,:)/norm(n(i,:));
            pos(i,:) = sum(vertices(index(:),:))/4;
            quiver3(pos(i,1),pos(i,2),pos(i,3),n(i,1),n(i,2),n(i,3),0.5);
        end
    
    end
    
    function [minDist, dist]= distToArena(cuboid,normal,point)
        % Calculate the distance from a point outside the cuboid to the center of the cuboid.
        % Input:
        %   cuboid: the handle of cuboid returned by the function patch().
        %   normal: the normal of faces.
        %   point:  the point outside the cuboid.
        % return
        %   minDist: The distance between the point and the surface of the cuboid,
        %           the direction along the line connecting the point and the center of the cuboid.
        %   dist:    The distance between the center of the cuboid and the surface of the cuboid,
        %           the direction along the line connecting the point and the center of the cuboid.
        faces = cuboid.Faces;
        minDist = inf;
    
        arenaCenter = sum(cuboid.Vertices(:,:))/size(cuboid.Vertices,1);
        for i = 1:size(faces,1)
            angle = getAngle(normal(i,:),point-arenaCenter);
            if angle<=90
                for j=1:4
                    faceCenter = sum(cuboid.Vertices(faces(i,:),:))/4;
                end
                distToFace = norm((faceCenter-arenaCenter))/cos(deg2rad(angle));
                dir = point-arenaCenter;
                dir = dir/norm(dir);
                onFace = dir*distToFace+arenaCenter;
                
                vertices = cuboid.Vertices(faces(i,:),:);
                x = [min(vertices(:,1)), max(vertices(:,1))];
                y = [min(vertices(:,2)), max(vertices(:,2))];
                z = [min(vertices(:,3)), max(vertices(:,3))];
                
                if onFace(1)>=x(1) && onFace(1)<=x(2) ...
                   && onFace(2)>=y(1) && onFace(2)<=y(2) ...
                   && onFace(3)>=z(1) && onFace(3)<=z(2)
                    minDist = norm(point-arenaCenter) - distToFace;
                    dist = distToFace;
                    return
                end
            end
        end
    end
    
    function angle = getAngle(A,B)
        angle = acos(dot(A,B)/(norm(A)*norm(B)))/pi*180;
    end

# 结果展示 #

显示立方体，并在每个面中心处将法向量画出。  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70]

[MATLAB_patch]: https://blog.csdn.net/qq_16775293/article/details/100982159
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70]: https://img-blog.csdnimg.cn/201909241103104.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz,size_16,color_FFFFFF,t_70