Find the shortest path from V1 to V2 along surface
The plane is defined by (V2 - V1) & the normal direction 'xn'
The path is cross section of surface and plane
Vpath = plane_cross_section(V,F,V1,V2,xx,dmax)
--- Input
V : voxcel coordinate of the surface [Nvertex x 3]
F : patch index of the surface [Npatch x 3]
V1 : start point on the surface
V2 : end point on the surface
xn : mean normal direction at V1 & V2
--- Output
Vpath : set of point coordinates along shotrest path (Npath x 3)
dmax : search distance from cut plane (= 0.05 m)
maximum edge length of surface patch
--- Intersection point at Z = 0
Line : V1 = [X1 Y1 Z1] to V2 = [X2 Y2 Z2]
: V = (V1 - V2) * t + V2
z = 0 :
x = (X1*Z2 - X2*Z1)/(Z2 - Z1)
y = (Y1*Z2 - Y2*Z1)/(Z2 - Z1)
2015-5-13 M. Sato
Copyright (C) 2011, ATR All Rights Reserved.
License : New BSD License(see VBMEG_LICENSE.txt)0001 function Vpath = plane_cross_section(V,F,V1,V2,xx,dmax) 0002 % Find the shortest path from V1 to V2 along surface 0003 % The plane is defined by (V2 - V1) & the normal direction 'xn' 0004 % The path is cross section of surface and plane 0005 % 0006 % Vpath = plane_cross_section(V,F,V1,V2,xx,dmax) 0007 % 0008 % --- Input 0009 % V : voxcel coordinate of the surface [Nvertex x 3] 0010 % F : patch index of the surface [Npatch x 3] 0011 % V1 : start point on the surface 0012 % V2 : end point on the surface 0013 % xn : mean normal direction at V1 & V2 0014 % --- Output 0015 % Vpath : set of point coordinates along shotrest path (Npath x 3) 0016 % dmax : search distance from cut plane (= 0.05 m) 0017 % maximum edge length of surface patch 0018 % --- Intersection point at Z = 0 0019 % Line : V1 = [X1 Y1 Z1] to V2 = [X2 Y2 Z2] 0020 % : V = (V1 - V2) * t + V2 0021 % z = 0 : 0022 % x = (X1*Z2 - X2*Z1)/(Z2 - Z1) 0023 % y = (Y1*Z2 - Y2*Z1)/(Z2 - Z1) 0024 % 0025 % 2015-5-13 M. Sato 0026 % 0027 % Copyright (C) 2011, ATR All Rights Reserved. 0028 % License : New BSD License(see VBMEG_LICENSE.txt) 0029 0030 if ~exist('dmax','var'), dmax = 0.05; end; 0031 0032 % Make local XYZ coordinate (origine = V1) 0033 Xdir = V2 -V1; 0034 Xdir = Xdir/sqrt(sum((Xdir.^2))); 0035 Zdir = vb_cross2(Xdir,xx); 0036 Zdir = Zdir/sqrt(sum((Zdir.^2))); 0037 Ydir = vb_cross2(Zdir,Xdir); 0038 Ydir = Ydir/sqrt(sum((Ydir.^2))); 0039 0040 % X coordinates of start & end points 0041 x1 = 0; 0042 x2 = (V2 -V1)*Xdir(:); 0043 0044 % rotation matrix 0045 Rot = [Xdir(:), Ydir(:), Zdir(:)]; 0046 0047 % Coordinate transform 0048 % XYZ = (V - V1)*Rot 0049 0050 XYZ = vb_repadd(V, - V1)*Rot; 0051 X = XYZ(:,1); 0052 Y = XYZ(:,2); 0053 Z = XYZ(:,3); 0054 0055 % 0056 % ---- Find set of intersection points at Z = 0 0057 % 0058 0059 % The plane is defined by Z = 0 0060 % Start & End points are [0, 0, 0] & [x2, 0, 0] 0061 0062 % Flag indicate upper & lower side of plane within dmax 0063 Uix = (Z >= 0 & Z <= dmax); % flag of upper side of plane 0064 Lix = (Z < 0 & Z >= -dmax);% flag of lower side of plane 0065 0066 % Flag for patch index 0067 FUix = Uix(F); 0068 FLix = Lix(F); 0069 0070 % Patch include both upper and lower side of plane within dmax 0071 Flist = find(sum(FUix,2) > 0 & sum(FLix,2) > 0); 0072 Nlist = length(Flist); 0073 0074 x = []; % x-coordinate to intersect the plane Z = 0 0075 y = []; % y-coordinate to intersect the plane Z = 0 0076 0077 % Loop for patch include both upper and lower side of plane 0078 for j=1:Nlist 0079 n = Flist(j); % patch id 0080 id = F(n,:); % vertex index of patch 0081 up = FUix(n,:); % Upper side flag of each vertex in the n-th patch 0082 dn = FLix(n,:); % Lower side flag of each vertex in the n-th patch 0083 0084 % Check two vertex of edge are in opposite side 0085 % : Check edge intersect the plane Z = 0 0086 % and add intersection points to the list 0087 if (up(1)*dn(2) + up(2)*dn(1)) > 0, 0088 % Two vertex points of edge 0089 X1 = X(id(1)); X2 = X(id(2)); 0090 Y1 = Y(id(1)); Y2 = Y(id(2)); 0091 Z1 = Z(id(1)); Z2 = Z(id(2)); 0092 0093 if Z1 == Z2, 0094 % line is on the Z = 0 plane 0095 x = [x; X1; X2]; 0096 y = [y; Y1; Y2]; 0097 else 0098 % Intersection point at Z = 0 0099 x = [x; (X1*Z2 - X2*Z1)/(Z2 - Z1)]; 0100 y = [y; (Y1*Z2 - Y2*Z1)/(Z2 - Z1)]; 0101 end 0102 end 0103 0104 if (up(2)*dn(3) + up(3)*dn(2)) > 0, 0105 % Two vertex points of edge 0106 X1 = X(id(2)); X2 = X(id(3)); 0107 Y1 = Y(id(2)); Y2 = Y(id(3)); 0108 Z1 = Z(id(2)); Z2 = Z(id(3)); 0109 0110 if Z1 == Z2, 0111 % line is on the Z = 0 plane 0112 x = [x; X1; X2]; 0113 y = [y; Y1; Y2]; 0114 else 0115 % Intersection point at Z = 0 0116 x = [x; (X1*Z2 - X2*Z1)/(Z2 - Z1)]; 0117 y = [y; (Y1*Z2 - Y2*Z1)/(Z2 - Z1)]; 0118 end 0119 end 0120 0121 if (up(3)*dn(1) + up(1)*dn(3)) > 0, 0122 % Two vertex points of edge 0123 X1 = X(id(3)); X2 = X(id(1)); 0124 Y1 = Y(id(3)); Y2 = Y(id(1)); 0125 Z1 = Z(id(3)); Z2 = Z(id(1)); 0126 0127 if Z1 == Z2, 0128 % line is on the Z = 0 plane 0129 x = [x; X1; X2]; 0130 y = [y; Y1; Y2]; 0131 else 0132 % Intersection point at Z = 0 0133 x = [x; (X1*Z2 - X2*Z1)/(Z2 - Z1)]; 0134 y = [y; (Y1*Z2 - Y2*Z1)/(Z2 - Z1)]; 0135 end 0136 end 0137 end 0138 0139 NP = length(x); 0140 0141 % 0142 % ---- Search path from V1 to V2 0143 % 0144 0145 % start point = V1 = [0 0 0] 0146 xp = 0; 0147 yp = 0; 0148 0149 xpath = xp; 0150 ypath = yp; 0151 dpath = []; 0152 0153 % extract self point 0154 dx = (x - xp).^2 + (y - yp).^2; 0155 [dmin, j0] = min(dx); % dmin = 0 0156 0157 % remove self point from list 0158 x(j0(1)) = []; 0159 y(j0(1)) = []; 0160 0161 % Find path from [0 0] to [x2 0] 0162 for n = 1:NP 0163 % select direction 0164 ix = find(x > xp); 0165 % find nearest point 0166 dx = (x(ix) - xp).^2 + (y(ix) - yp).^2; 0167 [dmin, j0] = min(dx); 0168 j0 = ix(j0(1)); 0169 0170 xp = x(j0); 0171 yp = y(j0); 0172 0173 x(j0) = []; 0174 y(j0) = []; 0175 0176 % add nearest point 0177 xpath = [xpath; xp]; 0178 ypath = [ypath; yp]; 0179 dpath = [dpath; sqrt(dmin)]; 0180 0181 if abs(xp-x2)<eps & abs(yp-0)<eps, break; end; 0182 end 0183 0184 % Back Coordinate transform 0185 % XYZ = (V - V1)*Rot 0186 % V = XYZ*R' + V1 0187 0188 Npath = length(xpath); 0189 xyz = [xpath ypath zeros(Npath,1)]; 0190 Vpath = xyz*Rot' ; 0191 Vpath = vb_repadd(Vpath, V1);