繪製立方體
繪製立方體主要使用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
結果展示
顯示立方體，並在每個面中心處將法向量畫出。
