XerxesZorgon/meshFunctions.m

## meshFunctions.m
% Opens a .ply mesh file, creates mesh structure with faces, vertices
% Requires plyread function:
% https://www.mathworks.com/matlabcentral/fileexchange/47484-plyread-m?s_tid=srchtitle
% https://www.mathworks.com/matlabcentral/mlc-downloads/downloads/submissions/47484/versions/1/previews/plyread.m/index.html
%
%
% Input(s)
%   plyFile: Name of .ply file to read (inc. path)
%
% Output(s)
%   mesh: Mesh structure with
%     faces: Indices of vertices making each triangular face
%     verts: Coordinates of vertices (x,y,z)
%
% Example:
% plyfile = '..\Overhead mesh\Vertices_mesh25D_red.ply';
% mesh = openMesh(plyfile);
%
% See also:
%
%
% Dependencies: plyread

%
%
% Written by: John Peach 02-Mar-2021
% Wild Peaches
%
% Revisions:


function mesh = openMesh(plyfile)
    % Read faces and vertices
    [faces,verts] = plyread(plyfile,'tri');

    % Add to mesh structure
    mesh.verts = verts;
    mesh.faces = faces;

endfunction

%------------------------------------------------------------------------------------------------

function [Elements,varargout] = plyread(Path,Str)
%PLYREAD   Read a PLY 3D data file.
%   [DATA,COMMENTS] = PLYREAD(FILENAME) reads a version 1.0 PLY file
%   FILENAME and returns a structure DATA.  The fields in this structure
%   are defined by the PLY header; each element type is a field and each
%   element property is a subfield.  If the file contains any comments,
%   they are returned in a cell string array COMMENTS.
%
%   [TRI,PTS] = PLYREAD(FILENAME,'tri') or
%   [TRI,PTS,DATA,COMMENTS] = PLYREAD(FILENAME,'tri') converts vertex
%   and face data into triangular connectivity and vertex arrays.  The
%   mesh can then be displayed using the TRISURF command.
%
%   Note: This function is slow for large mesh files (+50K faces),
%   especially when reading data with list type properties.
%
%   Example:
%   [Tri,Pts] = PLYREAD('cow.ply','tri');
%   trisurf(Tri,Pts(:,1),Pts(:,2),Pts(:,3));
%   colormap(gray); axis equal;
%
%   See also: PLYWRITE

% Pascal Getreuer 2004

[fid,Msg] = fopen(Path,'rt');	% open file in read text mode

if fid == -1, error(Msg); end

Buf = fscanf(fid,'%s',1);
if ~strcmp(Buf,'ply')
   fclose(fid);
   error('Not a PLY file.');
end


%%% read header %%%

Position = ftell(fid);
Format = '';
NumComments = 0;
Comments = {};				% for storing any file comments
NumElements = 0;
NumProperties = 0;
Elements = [];				% structure for holding the element data
ElementCount = [];		% number of each type of element in file
PropertyTypes = [];		% corresponding structure recording property types
ElementNames = {};		% list of element names in the order they are stored in the file
PropertyNames = [];		% structure of lists of property names

while 1
   Buf = fgetl(fid);   								% read one line from file
   BufRem = Buf;
   Token = {};
   Count = 0;

   while ~isempty(BufRem)								% split line into tokens
      [tmp,BufRem] = strtok(BufRem);

      if ~isempty(tmp)
         Count = Count + 1;							% count tokens
         Token{Count} = tmp;
      end
   end

   if Count 		% parse line
      switch lower(Token{1})
      case 'format'		% read data format
         if Count >= 2
            Format = lower(Token{2});

            if Count == 3 & ~strcmp(Token{3},'1.0')
               fclose(fid);
               error('Only PLY format version 1.0 supported.');
            end
         end
      case 'comment'		% read file comment
         NumComments = NumComments + 1;
         Comments{NumComments} = '';
         for i = 2:Count
            Comments{NumComments} = [Comments{NumComments},Token{i},' '];
         end
      case 'element'		% element name
         if Count >= 3
            if isfield(Elements,Token{2})
               fclose(fid);
               error(['Duplicate element name, ''',Token{2},'''.']);
            end

            NumElements = NumElements + 1;
            NumProperties = 0;
   	      Elements = setfield(Elements,Token{2},[]);
            PropertyTypes = setfield(PropertyTypes,Token{2},[]);
            ElementNames{NumElements} = Token{2};
            PropertyNames = setfield(PropertyNames,Token{2},{});
            CurElement = Token{2};
            ElementCount(NumElements) = str2double(Token{3});

            if isnan(ElementCount(NumElements))
               fclose(fid);
               error(['Bad element definition: ',Buf]);
            end
         else
            error(['Bad element definition: ',Buf]);
         end
      case 'property'	% element property
         if ~isempty(CurElement) & Count >= 3
            NumProperties = NumProperties + 1;
            eval(['tmp=isfield(Elements.',CurElement,',Token{Count});'],...
               'fclose(fid);error([''Error reading property: '',Buf])');

            if tmp
               error(['Duplicate property name, ''',CurElement,'.',Token{2},'''.']);
            end

            % add property subfield to Elements
            eval(['Elements.',CurElement,'.',Token{Count},'=[];'], ...
               'fclose(fid);error([''Error reading property: '',Buf])');
            % add property subfield to PropertyTypes and save type
            eval(['PropertyTypes.',CurElement,'.',Token{Count},'={Token{2:Count-1}};'], ...
               'fclose(fid);error([''Error reading property: '',Buf])');
            % record property name order
            eval(['PropertyNames.',CurElement,'{NumProperties}=Token{Count};'], ...
               'fclose(fid);error([''Error reading property: '',Buf])');
         else
            fclose(fid);

            if isempty(CurElement)
               error(['Property definition without element definition: ',Buf]);
            else
               error(['Bad property definition: ',Buf]);
            end
         end
      case 'end_header'	% end of header, break from while loop
         break;
      end
   end
end

%%% set reading for specified data format %%%

if isempty(Format)
	warning('Data format unspecified, assuming ASCII.');
   Format = 'ascii';
end

switch Format
case 'ascii'
   Format = 0;
case 'binary_little_endian'
   Format = 1;
case 'binary_big_endian'
   Format = 2;
otherwise
   fclose(fid);
   error(['Data format ''',Format,''' not supported.']);
end

if ~Format
   Buf = fscanf(fid,'%f');		% read the rest of the file as ASCII data
   BufOff = 1;
else
   % reopen the file in read binary mode
   fclose(fid);

   if Format == 1
      fid = fopen(Path,'r','ieee-le.l64');		% little endian
   else
      fid = fopen(Path,'r','ieee-be.l64');		% big endian
   end

   % find the end of the header again (using ftell on the old handle doesn't give the correct position)
   BufSize = 8192;
   Buf = [blanks(10),char(fread(fid,BufSize,'uchar')')];
   i = [];
   tmp = -11;

   while isempty(i)
   	i = findstr(Buf,['end_header',13,10]);			% look for end_header + CR/LF
   	i = [i,findstr(Buf,['end_header',10])];		% look for end_header + LF

      if isempty(i)
         tmp = tmp + BufSize;
         Buf = [Buf(BufSize+1:BufSize+10),char(fread(fid,BufSize,'uchar')')];
      end
   end

   % seek to just after the line feed
   fseek(fid,i + tmp + 11 + (Buf(i + 10) == 13),-1);
end


%%% read element data %%%

% PLY and MATLAB data types (for fread)
PlyTypeNames = {'char','uchar','short','ushort','int','uint','float','double', ...
   'char8','uchar8','short16','ushort16','int32','uint32','float32','double64'};
MatlabTypeNames = {'schar','uchar','int16','uint16','int32','uint32','single','double'};
SizeOf = [1,1,2,2,4,4,4,8];	% size in bytes of each type

for i = 1:NumElements
   % get current element property information
   eval(['CurPropertyNames=PropertyNames.',ElementNames{i},';']);
   eval(['CurPropertyTypes=PropertyTypes.',ElementNames{i},';']);
   NumProperties = size(CurPropertyNames,2);

   %fprintf('Reading %s...\n',ElementNames{i});

   if ~Format	%%% read ASCII data %%%
      for j = 1:NumProperties
         Token = getfield(CurPropertyTypes,CurPropertyNames{j});

         if strcmpi(Token{1},'list')
            Type(j) = 1;
         else
            Type(j) = 0;
			end
      end

      % parse buffer
      if ~any(Type)
         % no list types
         Data = reshape(Buf(BufOff:BufOff+ElementCount(i)*NumProperties-1),NumProperties,ElementCount(i))';
         BufOff = BufOff + ElementCount(i)*NumProperties;
      else
         ListData = cell(NumProperties,1);

         for k = 1:NumProperties
            ListData{k} = cell(ElementCount(i),1);
         end

         % list type
		   for j = 1:ElementCount(i)
   	      for k = 1:NumProperties
      	      if ~Type(k)
         	      Data(j,k) = Buf(BufOff);
            	   BufOff = BufOff + 1;
	            else
   	            tmp = Buf(BufOff);
      	         ListData{k}{j} = Buf(BufOff+(1:tmp))';
         	      BufOff = BufOff + tmp + 1;
            	end
            end
         end
      end
   else		%%% read binary data %%%
      % translate PLY data type names to MATLAB data type names
      ListFlag = 0;		% = 1 if there is a list type
      SameFlag = 1;     % = 1 if all types are the same

      for j = 1:NumProperties
         Token = getfield(CurPropertyTypes,CurPropertyNames{j});

         if ~strcmp(Token{1},'list')			% non-list type
	         tmp = rem(strmatch(Token{1},PlyTypeNames,'exact')-1,8)+1;

            if ~isempty(tmp)
               TypeSize(j) = SizeOf(tmp);
               Type{j} = MatlabTypeNames{tmp};
               TypeSize2(j) = 0;
               Type2{j} = '';

               SameFlag = SameFlag & strcmp(Type{1},Type{j});
	         else
   	         fclose(fid);
               error(['Unknown property data type, ''',Token{1},''', in ', ...
                     ElementNames{i},'.',CurPropertyNames{j},'.']);
         	end
         else											% list type
            if length(Token) == 3
               ListFlag = 1;
               SameFlag = 0;
               tmp = rem(strmatch(Token{2},PlyTypeNames,'exact')-1,8)+1;
               tmp2 = rem(strmatch(Token{3},PlyTypeNames,'exact')-1,8)+1;

               if ~isempty(tmp) & ~isempty(tmp2)
                  TypeSize(j) = SizeOf(tmp);
                  Type{j} = MatlabTypeNames{tmp};
                  TypeSize2(j) = SizeOf(tmp2);
                  Type2{j} = MatlabTypeNames{tmp2};
	   	      else
   	   	      fclose(fid);
               	error(['Unknown property data type, ''list ',Token{2},' ',Token{3},''', in ', ...
                        ElementNames{i},'.',CurPropertyNames{j},'.']);
               end
            else
               fclose(fid);
               error(['Invalid list syntax in ',ElementNames{i},'.',CurPropertyNames{j},'.']);
            end
         end
      end

      % read file
      if ~ListFlag
         if SameFlag
            % no list types, all the same type (fast)
            Data = fread(fid,[NumProperties,ElementCount(i)],Type{1})';
         else
            % no list types, mixed type
            Data = zeros(ElementCount(i),NumProperties);

         	for j = 1:ElementCount(i)
        			for k = 1:NumProperties
               	Data(j,k) = fread(fid,1,Type{k});
              	end
         	end
         end
      else
         ListData = cell(NumProperties,1);

         for k = 1:NumProperties
            ListData{k} = cell(ElementCount(i),1);
         end

         if NumProperties == 1
            BufSize = 512;
            SkipNum = 4;
            j = 0;

            % list type, one property (fast if lists are usually the same length)
            while j < ElementCount(i)
               BufSize = min(ElementCount(i)-j,BufSize);
               Position = ftell(fid);
               % read in BufSize count values, assuming all counts = SkipNum
               [Buf,BufSize] = fread(fid,BufSize,Type{1},SkipNum*TypeSize2(1));
               Miss = find(Buf ~= SkipNum);					% find first count that is not SkipNum
               fseek(fid,Position + TypeSize(1),-1); 		% seek back to after first count

               if isempty(Miss)									% all counts are SkipNum
                  Buf = fread(fid,[SkipNum,BufSize],[int2str(SkipNum),'*',Type2{1}],TypeSize(1))';
                  fseek(fid,-TypeSize(1),0); 				% undo last skip

                  for k = 1:BufSize
                     ListData{1}{j+k} = Buf(k,:);
                  end

                  j = j + BufSize;
                  BufSize = floor(1.5*BufSize);
               else
                  if Miss(1) > 1									% some counts are SkipNum
                     Buf2 = fread(fid,[SkipNum,Miss(1)-1],[int2str(SkipNum),'*',Type2{1}],TypeSize(1))';

                     for k = 1:Miss(1)-1
                        ListData{1}{j+k} = Buf2(k,:);
                     end

                     j = j + k;
                  end

                  % read in the list with the missed count
                  SkipNum = Buf(Miss(1));
                  j = j + 1;
                  ListData{1}{j} = fread(fid,[1,SkipNum],Type2{1});
                  BufSize = ceil(0.6*BufSize);
               end
            end
         else
            % list type(s), multiple properties (slow)
            Data = zeros(ElementCount(i),NumProperties);

            for j = 1:ElementCount(i)
         		for k = 1:NumProperties
            		if isempty(Type2{k})
               		Data(j,k) = fread(fid,1,Type{k});
            		else
               		tmp = fread(fid,1,Type{k});
               		ListData{k}{j} = fread(fid,[1,tmp],Type2{k});
		            end
      		   end
      		end
         end
      end
   end

   % put data into Elements structure
   for k = 1:NumProperties
   	if (~Format & ~Type(k)) | (Format & isempty(Type2{k}))
      	eval(['Elements.',ElementNames{i},'.',CurPropertyNames{k},'=Data(:,k);']);
      else
      	eval(['Elements.',ElementNames{i},'.',CurPropertyNames{k},'=ListData{k};']);
		end
   end
end

clear Data ListData;
fclose(fid);

if (nargin > 1 & strcmpi(Str,'Tri')) | nargout > 2
   % find vertex element field
   Name = {'vertex','Vertex','point','Point','pts','Pts'};
   Names = [];

   for i = 1:length(Name)
      if any(strcmp(ElementNames,Name{i}))
         Names = getfield(PropertyNames,Name{i});
         Name = Name{i};
         break;
      end
   end

   if any(strcmp(Names,'x')) & any(strcmp(Names,'y')) & any(strcmp(Names,'z'))
      eval(['varargout{1}=[Elements.',Name,'.x,Elements.',Name,'.y,Elements.',Name,'.z];']);
   else
      varargout{1} = zeros(1,3);
	end

   varargout{2} = Elements;
   varargout{3} = Comments;
   Elements = [];

   % find face element field
   Name = {'face','Face','poly','Poly','tri','Tri'};
   Names = [];

   for i = 1:length(Name)
      if any(strcmp(ElementNames,Name{i}))
         Names = getfield(PropertyNames,Name{i});
         Name = Name{i};
         break;
      end
   end

   if ~isempty(Names)
      % find vertex indices property subfield
	   PropertyName = {'vertex_indices','vertex_indexes','vertex_index','indices','indexes'};

   	for i = 1:length(PropertyName)
      	if any(strcmp(Names,PropertyName{i}))
         	PropertyName = PropertyName{i};
	         break;
   	   end
      end

      if ~iscell(PropertyName)
         % convert face index lists to triangular connectivity
         eval(['FaceIndices=varargout{2}.',Name,'.',PropertyName,';']);
  			N = length(FaceIndices);
   		Elements = zeros(N*2,3);
   		Extra = 0;

			for k = 1:N
   			Elements(k,:) = FaceIndices{k}(1:3);

   			for j = 4:length(FaceIndices{k})
      			Extra = Extra + 1;
	      		Elements(N + Extra,:) = [Elements(k,[1,j-1]),FaceIndices{k}(j)];
   			end
         end
         Elements = Elements(1:N+Extra,:) + 1;
      end
   end
else
   varargout{1} = Comments;
end

%------------------------------------------------------------------------------------------------

% Returns the rotation axis and angle to map mesh coordinates into local
% The alignAxis is the coordinate axis that you want to map to. For example,
% if you want to map the local vertical to the z-axis, alignAxis = 'z'.
%
% Input(s)
%   P1, P2:  Points on roadway
%   alignAxis: Alignment axis to map ('x','y','z')
%
% Output(s)
%   omega: Rotation axis for rodrigues function
%   theta: Rotation angle
%
% Example:
% P2 = [-155.042770;77.017189;-59.749859];
% P3 = [-33.501743;-5.217833;0.409614];
% [omega,theta] = rotationParams(P2,P3,'z')
% omega =
%   0.9909
%   0.1345
%        0
%
% theta = 31.589
%
%
% See also: openMesh, volUnderRect, volUnderSlope
%
%
% Dependencies: unit

%
%
% Written by: John Peach 01-Mar-2021
% Wild Peaches
%
% Revisions:


function [omega,theta] = rotationParams(P1,P2,alignAxis)
    % Set rotation axis
    switch(lower(alignAxis))
        case{'x'}, ax = eye(3,1);
        case{'y'}, ax = circshift(eye(3,1),1);
        case{'z'}, ax = circshift(eye(3,1),2);
    otherwise
        error('alignAxis not recognized');
    endswitch

    % Unit vectors
    V1 = unit(P1);
    V2 = unit(P2);

    % Vector perpendicular to both V1 and V2
    V3 = unit(cross(V1,V2));

    % Rotation axis vector to map V3 into ax
    omega = unit(cross(V3,ax));

    % Rotation angle (rad)
    theta = acosd(dot(V3,ax));

endfunction

%------------------------------------------------------------------------------------------------

% Returns triangle faces contained in a rectangular region
%
%
% Input(s)
%   mesh: Mesh structure
%   lower_left, upper_right: Corners of rectangular region
%
% Output(s)
%   triInRect: Mesh faces with at least two vertices in the rectangle
%   inRectPts: Vertices contained in the rectangle
%
% Example:
% Select two points in CloudCompare:
% [14:19:04] [Picked]	- Tri#35648 (0.725444;-0.915855;0.159314)
% [14:19:14] [Picked]	- Tri#4706892 (199.652069;13.617809;-0.398731)
% lower_left = [0.725444;-0.915855];
% upper_right = [199.652069;13.617809];
% triInRect = triInMesh(mesh,lower_left,upper_right);
%
% See also: openMesh
%
%
% Dependencies: None

%
%
% Written by: John Peach 02-Mar-2021
% Wild Peaches
%
% Revisions:


function [triInRect,inRectPts] = triInMesh(mesh,lower_left,upper_right)

    % Locate vertices contained in rectangle
    x0 = lower_left(1);
    y0 = lower_left(2);
    x1 = upper_right(1);
    y1 = upper_right(2);
    inRectPts = find(mesh.verts(:,1) >= x0 & mesh.verts(:,1) <= x1 & ...
                     mesh.verts(:,2) >= y0 & mesh.verts(:,2) <= y1);

    % Array of faces with vertices inside the rectangle
    inRect = ismember(mesh.faces,inRectPts);

    % Indices of faces with at least two vertices contained in the rectangle
    triInRect = find(sum(inRect,2) >= 2);

endfunction

%------------------------------------------------------------------------------------------------

% Array of the k nearest neighbors to a set of points Pts
%
%
% Input(s)
%   mesh: Mesh structure
%   Pts:  Points
%   k:    Number of nearest neighbors to each point in Pts
%
% Output(s)
%   nbrs: Array of mesh indices to nearest neighbors [nPts x k]
%
% Example:
% P1 = [16.314060;10.285256];
% P2 = [185.938141;9.857399];
% Pts = linspace(P1,P2,100)';
% nbrs = knnsearch(mesh,Pts,3);
%
% See also: meshCrossSect
%
%
% Dependencies: vnorm

%
%
% Written by: John Peach 02-Mar-2021
% Wild Peaches
%
% Revisions:


function nbrs = knnsearch(mesh,Pts,k)
    % Estimate mesh density
    mesh_min = min(mesh.verts);
    mesh_max = max(mesh.verts);
    d_mesh = mesh_max - mesh_min;
    dx = d_mesh(1);
    dy = d_mesh(2);

    % Neighboring points
    sigma = 3;
    nPts = size(Pts,1);
    nbrs = zeros(nPts,k);
    hdl = waitbar(0,'Finding neighbor points');
    for p = 1:nPts
        x0 = Pts(p,1) - sigma*dx;
        x1 = Pts(p,1) + sigma*dx;
        y0 = Pts(p,2) - sigma*dy;
        y1 = Pts(p,2) + sigma*dy;
        % Points within a rectangular region around a point
        local_nbrs = find(mesh.verts(:,1) >= x0 & mesh.verts(:,1) <= x1 & ...
                          mesh.verts(:,2) >= y0 & mesh.verts(:,2) <= y1);
        % Distances to the point from each neighbor
        D = vnorm(mesh.verts(local_nbrs,1:2) - Pts(p,1:2),2);
        [D,idx] = sort(D);
        nbrs(p,:) = local_nbrs(idx(1:k));
        % Update waitbar
        waitbar(p/nPts,hdl);
    endfor

    % Close the waitbar
    close(hdl);

endfunction

%------------------------------------------------------------------------------------------------

% Plots a cross section through a mesh
%
%
% Input(s)
%   mesh: Mesh structure
%   P1,P2:
%
% Output(s)
%   Plot of a cross section of the mesh between points P1 and P2
%
% Example:
% P1 = [16.314060;10.285256];
% P2 = [185.938141;9.857399];
% meshCrossSect(mesh,P1,P2)
%
% See also: openMesh, volUnderRect
%
%
% Dependencies: knnsearch, pubFig

%
%
% Written by: John Peach 02-Mar-2021
% Wild Peaches
%
% Revisions:


function meshCrossSect(mesh,P1,P2)

    % Linearly spaced points from P1 to P2
    n = 50;
    Pts = linspace(P1,P2,n)';

    % Find nearest neighbor to each point
    k = 1;
    nbrs = knnsearch(mesh,Pts,k);

    % Depth values at each neighbor point
    z = mesh.verts(nbrs,3);

    % Plot cross section
    x = vnorm(Pts,2);
    figure;
    plot(x,z);
    xlabel('Cross Section (ft)');
    ylabel('Depth (ft)');
    title('Mesh Section');
    pubFig;

endfunction

%------------------------------------------------------------------------------------------------

% Estimates the volume of a mesh under a rectangular area
%
%
% Input(s)
%   mesh: Mesh structure
%   lower_left, upper_right: Corners of rectangular region
%
%
% Output(s)
%   meshVol: Volume between rectangular plane and mesh surface
%
% Example:
% mesh = openMesh(plyfile);
% lower_left = [0.725444;-0.915855];
% upper_right = [199.652069;13.617809];
% meshVol = volUnderRect(mesh,,lower_left,upper_right)
% meshVol = 1.4788e+04
%
% See also: volUnderSlope, openMesh, triInMesh
%
%
% Dependencies: unit, vnorm, triInMesh

%
%
% Written by: John Peach 02-Mar-2021
% Wild Peaches
%
% Revisions:


function meshVol = volUnderRect(mesh,lower_left,upper_right)

    % Face indices contained in rectangular region
    triInRect = triInMesh(mesh,lower_left,upper_right);

    % Faces contained in the rectangle
    faceInRect = mesh.faces(triInRect,:);

    % Vertices of faces in rectangle
    P1 = mesh.verts(faceInRect(:,1),:);
    P2 = mesh.verts(faceInRect(:,2),:);
    P3 = mesh.verts(faceInRect(:,3),:);

    % Vectors from P1 - P2, P1 - P3
    V12 = P2 - P1;
    V13 = P3 - P1;
    V12(:,3) = 0;
    V13(:,3) = 0;

    % Cross product
    Vn = cross(V12,V13);

    % Face areas
    FA = vnorm(Vn,2)/2;

    % Approximate prism length
    depth = -median([P1(:,3) P2(:,3) P3(:,3)],2);
    L = max(depth,0);

    % Mesh volume is face area x prism length
    meshVol = sum(FA .* L);

endfunction

%------------------------------------------------------------------------------------------------

% Estimates the volume of a mesh under a sloped rectangular area
% The plane slopes at an angle of 45 degrees away from the edge of the road
%
%
%
% Input(s)
%   mesh: Mesh structure
%   lower_left, upper_right: Corners of rectangular region
%
%
% Output(s)
%   meshVol: Volume between sloping rectangular plane and mesh surface
%
%
% Example:
% volLeft = volUnderSlope(mesh,[0;-30],[180;-10])
% volRight = volUnderSlope(mesh,[0;32],[180;52])
%
% See also: volUnderRect, openMesh, triInMesh
%
%
% Dependencies: unit, vnorm, triInMesh

%
%
% Written by: John Peach 06-Mar-2021
% Wild Peaches
%
% Revisions:


function meshVol = volUnderSlope(mesh,lower_left,upper_right)

    % Road and shoulder widths
    roadWidth = 22;
    shoulderWidth = 10;

    % Faces indices contained in rectangular region
    triInRect = triInMesh(mesh,lower_left,upper_right);

    % Faces contained in the rectangle
    faceInRect = mesh.faces(triInRect,:);

    % Vertices of faces in rectangle
    P1 = mesh.verts(faceInRect(:,1),:);
    P2 = mesh.verts(faceInRect(:,2),:);
    P3 = mesh.verts(faceInRect(:,3),:);

    % Vectors from P1 - P2, P1 - P3
    V12 = P2 - P1;
    V13 = P3 - P1;
    V12(:,3) = 0;
    V13(:,3) = 0;

    % Cross product
    Vn = cross(V12,V13);

    % Face areas
    FA = vnorm(Vn,2)/2;

    % Approximate prism length
    depth = median([P1(:,3) P2(:,3) P3(:,3)],2);
    yCoord = mean([P1(:,2) P2(:,2) P3(:,2)],2);
    % Height of sloped rectangle depends on distance from shoulder edge
    if all(yCoord < 0)
        slopeHeight = yCoord + shoulderWidth;
    elseif all(yCoord > 0)
        slopeHeight = yCoord - (roadWidth + shoulderWidth);
    else
        error('Sloping rectangle must be entirely on one side of the road.');
    endif

    % Prism length is difference between slope and depth. If slope is less
    % than depth, set length to zero.
    L = max(slopeHeight - depth,0);

    % Mesh volume is face area x prism length
    meshVol = sum(FA .* L);


endfunction

%------------------------------------------------------------------------------------------------

% Returns the unit vector in the direction of input vector v
% If v is an array then unit returns an array of unit vectors along
% dimension d
%
%
% Input(s)
%    v:  Vector or array of vectors
%    d:  Dimension [optional, default = 2 if v is an array]
%
%
% Output(s)
%    u:  Unit vector, u = v/||v||
%
%
% Example:
% r = randi(10,5,3)
% r =
%      1     3     3
%     10     5     5
%      8    10     7
%      5     6     7
%      6     6     4
%
% unit(r,2) =
%     0.2294    0.6882    0.6882
%     0.8165    0.4082    0.4082
%     0.5482    0.6852    0.4796
%     0.4767    0.5721    0.6674
%     0.6396    0.6396    0.4264
%
%
% See also:
%
%
% Dependencies: vnorm

%
%
% Written by: John Peach 02-Mar-2021
% Wild Peaches
%
% Revisions:


function u = unit(v,d)

% Dimensions of v
[r,c] = size(v);
if ~any([r c] == 1)
    % v is an array of vectors
    if nargin == 1
        d = 2;
    end;
    unit_v = vnorm(v,d);
    if d == 1
    u = v ./ unit_v(ones(r,1),:);
    else
        u = v ./ unit_v(:,ones(1,c),:);
    end;
else
    % v is a vector
    if norm(v) > 0,
        u = v/norm(v);
    else
        u = v;
    end;
end;

%------------------------------------------------------------------------------------------------

% Returns the 2-norm of columns or rows of A
%
%
% Input(s)
%   A:   Matrix [m x n]
%   dim: Dimension to take norm
%
% Output(s)
%   V:   Vector of rows or columns of A
%
% Example:
% A = rand(10,5);
% V = vnorm(A,2);
%
% See also: vnorm_p
%
%
% Dependencies: None

%
%
% Written by: John Peach 02-Mar-2021
% Wild Peaches
%
% Revisions:


function V = vnorm(A,dim)

% Size of input array
[r,c] = size(A);

% Use 2nd dimension if only array is input
if nargin == 1
    dim = 2;
end

% Calculate 2-norm of rows or columns
if dim == 2
    V = zeros(r,1);
    for k = 1:r
        V(k) = norm(A(k,:));
    end
elseif dim == 1
    V = zeros(1,c);
    for k = 1:c
        V(k) = norm(A(:,k));
    end
else
    error('dim must be either 1 or 2');
end

%------------------------------------------------------------------------------------------------

% Modifies figures to get ready for publication
%
%
% Input(s)
%   None
%
% Output(s)
%   Current figure revised, suitable for publication
%
% Example:
% t = linspace(0,2*pi);
% y = sin(t);
% figure;
% plot(t,y);
% xlabel('Time')
% ylabel('sin(t)')
% title('Sine function')
% pubFig
%
% See also:
%
%
% Dependencies: None

%
%
% Written by: John Peach 20-Aug-2020
% Wild Peaches
%
% Revisions:


function pubFig
    % Set standard units
    figUnits=get(gcf,'Units');
    set(gcf,'Units','inches');

    % Linewidth, fonts, figure dimensions
    linewidth = 1.5;
    axisFont  = 16; %12;
    labelFont = 20; %14;
    titleFont = 24; %16;
    fontName  = 'Arial';
    figPos    = get(gcf,'Position');

    % Modify text
    texts = findall(gcf,'Type','text');
    for k = 1:numel(texts)
        textParent = get(texts(k),'Parent');
        if ~strcmp(get(textParent,'Tag'),'legend')
            set(texts(k), ...
                'FontSize',labelFont, ...
                'FontWeight','bold',...
                'FontName',fontName);
        endif
    endfor

    % Set line widths
    figLines = findall(gcf,'Type','line');
    for k = 1:numel(figLines)
        set(figLines(k),'LineWidth',linewidth);
    endfor

    % Adjust axes
    figAxes = findall(gcf,'Type','axes');
    for k = 1:numel(figAxes)
        set(figAxes(k), ...
            'LineWidth',linewidth, ...
            'Fontsize',axisFont,...
            'FontWeight','bold', ...
            'FontName',fontName);
    endfor

    % Correct figure size
    currPos = get(gcf,'Position');
    newPos  = [currPos(1:2) - (figPos(3:4) - currPos(3:4)) figPos(3:4)];
    set(gcf,'Position',newPos)
    set(gcf,'PaperPosition',[1 1 figPos(3:4)])
    set(gcf,'Units',figUnits)

endfunction