inpolyhedron.m

function IN = inpolyhedron(varargin)
%INPOLYHEDRON  Tests if points are inside a 3D triangulated (faces/vertices) surface
%   BY CONVENTION, SURFACE NORMALS SHOULD POINT OUT from the object. (see
%   FLIPNORMALS option below for details)
%
%   IN = INPOLYHEDRON(FV,QPTS) tests if the query points (QPTS) are inside the
%   patch/surface/polyhedron defined by FV (a structure with fields 'vertices' and
%   'faces'). QPTS is an N-by-3 set of XYZ coordinates. IN is an N-by-1 logical
%   vector which will be TRUE for each query point inside the surface.
%
%   INPOLYHEDRON(FACES,VERTICES,...) takes faces/vertices separately, rather than in
%   an FV structure.
%
%   IN = INPOLYHEDRON(..., X, Y, Z) voxelises a mask of 3D gridded query points
%   rather than an N-by-3 array of points. X, Y, and Z coordinates of the grid
%   supplied in XVEC, YVEC, and ZVEC respectively. IN will return as a 3D logical
%   volume with SIZE(IN) = [LENGTH(YVEC) LENGTH(XVEC) LENGTH(ZVEC)], equivalent to
%   syntax used by MESHGRID. INPOLYHEDRON handles this input faster and with a lower 
%   memory footprint than using MESHGRID to make full X, Y, Z query points matrices.
%
%   INPOLYHEDRON(...,'PropertyName',VALUE,'PropertyName',VALUE,...) tests query
%   points using the following optional property values:
%
%   TOL           - Tolerance on the tests for "inside" the surface. You can think of
%   tol as the distance a point may possibly lie above/below the surface, and still
%   be perceived as on the surface. Due to numerical rounding nothing can ever be
%   done exactly here. Defaults to ZERO. Note that in the current implementation TOL
%   only affects points lying above/below a surface triangle (in the Z-direction).
%   Points coincident with a vertex in the XY plane are considered INside the surface.
%   More formal rules can be implemented with input/feedback from users.
%
%   GRIDSIZE      - Internally, INPOLYHEDRON uses a divide-and-conquer algorithm to
%   split all faces into a chessboard-like grid of GRIDSIZE-by-GRIDSIZE regions.
%   Performance will be a tradeoff between a small GRIDSIZE (few iterations, more
%   data per iteration) and a large GRIDSIZE (many iterations of small data
%   calculations). The sweet-spot has been experimentally determined (on a win64
%   system) to be correlated with the number of faces/vertices. You can overwrite
%   this automatically computed choice by specifying a GRIDSIZE parameter.
%
%   FACENORMALS   - By default, the normals to the FACE triangles are computed as the
%   cross-product of the first two triangle edges. You may optionally specify face
%   normals here if they have been pre-computed.
%
%   FLIPNORMALS   - (Defaults FALSE). To match a wider convention, triangle
%   face normals are presumed to point OUT from the object's surface. If
%   your surface normals are defined pointing IN, then you should set the
%   FLIPNORMALS option to TRUE to use the reverse of this convention.
%
%   Example:
%       tmpvol = zeros(20,20,20);       % Empty voxel volume
%       tmpvol(5:15,8:12,8:12) = 1;     % Turn some voxels on
%       tmpvol(8:12,5:15,8:12) = 1;
%       tmpvol(8:12,8:12,5:15) = 1;
%       fv = isosurface(tmpvol, 0.99);  % Create the patch object
%       fv.faces = fliplr(fv.faces);    % Ensure normals point OUT
%       % Test SCATTERED query points
%       pts = rand(200,3)*12 + 4;       % Make some query points
%       in = inpolyhedron(fv, pts);     % Test which are inside the patch
%       figure, hold on, view(3)        % Display the result
%       patch(fv,'FaceColor','g','FaceAlpha',0.2)
%       plot3(pts(in,1),pts(in,2),pts(in,3),'bo','MarkerFaceColor','b')
%       plot3(pts(~in,1),pts(~in,2),pts(~in,3),'ro'), axis image
%       % Test STRUCTURED GRID of query points
%       gridLocs = 3:2.1:19;
%       [x,y,z] = meshgrid(gridLocs,gridLocs,gridLocs);
%       in = inpolyhedron(fv, gridLocs,gridLocs,gridLocs);
%       figure, hold on, view(3)        % Display the result
%       patch(fv,'FaceColor','g','FaceAlpha',0.2)
%       plot3(x(in), y(in), z(in),'bo','MarkerFaceColor','b')
%       plot3(x(~in),y(~in),z(~in),'ro'), axis image
%
% See also: UNIFYMESHNORMALS (on the <a href="http://www.mathworks.com/matlabcentral/fileexchange/loadFile.do?objectId=43013">file exchange</a>)

% TODO-list
% - Optmise overall memory footprint. (need examples with MEM errors)
% - Implement an "ignore these" step to speed up calculations for:
%     * Query points outside the convex hull of the faces/vertices input
% - Get a better/best gridSize calculation. User feedback?
% - Detect cases where X-rays or Y-rays would be better than Z-rays?

%
%   Author: Sven Holcombe
% - 10 Jun 2012: Version 1.0
% - 28 Aug 2012: Version 1.1 - Speedup using accumarray
% - 07 Nov 2012: Version 2.0 - BEHAVIOUR CHANGE
%                Query points coincident with a VERTEX are now IN an XY triangle
% - 18 Aug 2013: Version 2.1 - Gridded query point handling with low memory footprint.
% - 10 Sep 2013: Version 3.0 - BEHAVIOUR CHANGE 
%                NEW CONVENTION ADOPTED to expect face normals pointing IN
%                Vertically oriented faces are now ignored. Speeds up
%                computation and fixes bug where presence of vertical faces
%                produced NaN distance from a query pt to facet, making all
%                query points under facet erroneously NOT IN polyhedron.
% - 25 Sep 2013: Version 3.1 - Dropped nested unique call which was made
%                mostly redundant via v2.1 gridded point handling. Also
%                refreshed grid size selection via optimisation.
% - 25 Feb 2014: Version 3.2 - Fixed indeterminate behaviour for query 
%                points *exactly* in line with an "overhanging" vertex.
%%

% FACETS is an unpacked arrangement of faces/vertices. It is [3-by-3-by-N],
% with 3 1-by-3 XYZ coordinates of N faces.
[facets, qPts, options] = parseInputs(varargin{:});
numFaces = size(facets,3);
if ~options.griddedInput            % SCATTERED QUERY POINTS
    numQPoints = size(qPts,1);
else                                % STRUCTURED QUERY POINTS
    numQPoints = prod(cellfun(@numel,qPts(1:2)));
end

% Precompute 3d normals to all facets (triangles). Do this via the cross
% product of the first edge vector with the second. Normalise the result.
allEdgeVecs = facets([2 3 1],:,:) - facets(:,:,:);
if isempty(options.facenormals)
    allFacetNormals =  bsxfun(@times, allEdgeVecs(1,[2 3 1],:), allEdgeVecs(2,[3 1 2],:)) - ...
        bsxfun(@times, allEdgeVecs(2,[2 3 1],:), allEdgeVecs(1,[3 1 2],:));
    allFacetNormals = bsxfun(@rdivide, allFacetNormals, sqrt(sum(allFacetNormals.^2,2)));
else
    allFacetNormals = permute(options.facenormals,[3 2 1]);
end
if options.flipnormals
    allFacetNormals = -allFacetNormals;
end
% We use a Z-ray intersection so we don't even need to consider facets that
% are purely vertically oriented (have zero Z-component).
isFacetUseful = allFacetNormals(:,3,:) ~= 0;

%% Setup grid referencing system
% Function speed can be thought of as a function of grid size. A small number of grid
% squares means iterating over fewer regions (good) but with more faces/qPts to
% consider each time (bad). For any given mesh/queryPt configuration, there will be a
% sweet spot that minimises computation time. There will also be a constraint from
% memory available - low grid sizes means considering many queryPt/faces at once,
% which will require a larger memory footprint. Here we will let the user specify
% gridsize directly, or we will estimate the optimum size based on prior testing.
if ~isempty(options.gridsize)
    gridSize = options.gridsize;
else
    % Coefficients (with 95% confidence bounds):
    p00 =         -47;    p10 =       12.83;    p01 =       20.89;
    p20 =      0.7578;    p11 =      -6.511;    p02 =      -2.586;
    p30 =     -0.1802;    p21 =      0.2085;    p12 =      0.7521;
    p03 =     0.09984;    p40 =    0.005815;    p31 =    0.007775;
    p22 =    -0.02129;    p13 =    -0.02309;
    GSfit = @(x,y)p00 + p10*x + p01*y + p20*x^2 + p11*x*y + p02*y^2 + p30*x^3 + p21*x^2*y + p12*x*y^2 + p03*y^3 + p40*x^4 + p31*x^3*y + p22*x^2*y^2 + p13*x*y^3;
    gridSize = min(150 ,max(1, ceil(GSfit(log(numQPoints),log(numFaces)))));
    if isnan(gridSize), gridSize = 1; end
end

%% Find candidate qPts -> triangles pairs
% We have a large set of query points. For each query point, find potential
% triangles that would be pierced by vertical rays through the qPt. First,
% a simple filter by XY bounding box

% Calculate the bounding box of each facet
minFacetCoords = permute(min(facets(:,1:2,:),[],1),[3 2 1]);
maxFacetCoords = permute(max(facets(:,1:2,:),[],1),[3 2 1]);

% Set rescale values to rescale all vertices between 0(-eps) and 1(+eps)
scalingOffsetsXY = min(minFacetCoords,[],1) - eps;
scalingRangeXY = max(maxFacetCoords,[],1) - scalingOffsetsXY + 2*eps;

% Based on scaled min/max facet coords, get the [lowX lowY highX highY] "grid" index
% of all faces
lowToHighGridIdxs = floor(bsxfun(@rdivide, ...
    bsxfun(@minus, ... % Use min/max coordinates of each facet (+/- the tolerance)
    [minFacetCoords-options.tol maxFacetCoords+options.tol],...
    [scalingOffsetsXY scalingOffsetsXY]),...
    [scalingRangeXY scalingRangeXY]) * gridSize) + 1;

% Build a grid of cells. In each cell, place the facet indices that encroach into
% that grid region. Similarly, each query point will be assigned to a grid region.
% Note that query points will be assigned only one grid region, facets can cover many
% regions. Furthermore, we will add a tolerance to facet region assignment to ensure
% a query point will be compared to facets even if it falls only on the edge of a
% facet's bounding box, rather than inside it.
cells = cell(gridSize);
[unqLHgrids,junk,facetInds] = unique(lowToHighGridIdxs,'rows');
tmpInds = accumarray(facetInds(isFacetUseful),find(isFacetUseful),[size(unqLHgrids,1),1],@(x){x});
for xi = 1:gridSize
    xyMinMask = xi >= unqLHgrids(:,1) & xi <= unqLHgrids(:,3);
    for yi = 1:gridSize
        cells{yi,xi} = cat(1,tmpInds{xyMinMask & yi >= unqLHgrids(:,2) & yi <= unqLHgrids(:,4)});
        % The above line (with accumarray) is faster with equiv results than:
        % % cells{yi,xi} = find(ismember(facetInds, xyInds));
    end
end
% With large number of facets, memory may be important:
clear lowToHightGridIdxs LHgrids facetInds tmpInds xyMinMask minFacetCoords maxFacetCoords

%% Compute edge unit vectors and dot products

% Precompute the 2d unit vectors making up each facet's edges in the XY plane.
allEdgeUVecs = bsxfun(@rdivide, allEdgeVecs(:,1:2,:), sqrt(sum(allEdgeVecs(:,1:2,:).^2,2)));

% Precompute the inner product between edgeA.edgeC, edgeB.edgeA, edgeC.edgeB
allEdgeEdgeDotPs = sum(allEdgeUVecs .* -allEdgeUVecs([3 1 2],:,:),2) - 1e-9;

%% Gather XY query locations
% Since query points are most likely given as a (3D) grid of query locations, we only
% need to consider the unique XY locations when asking which facets a vertical ray
% through an XY location would pierce.
if ~options.griddedInput            % SCATTERED QUERY POINTS
    qPtsXY = @(varargin)qPts(:,1:2);
    qPtsXYZViaUnqIndice = @(ind)qPts(ind,:);
    outPxIndsViaUnqIndiceMask = @(ind,mask)ind(mask);
    outputSize = [size(qPts,1),1];
    reshapeINfcn = @(INMASK)INMASK;
    minFacetDistanceFcn = @minFacetToQptDistance;
else                                % STRUCTURED QUERY POINTS
    [xmat,ymat] = meshgrid(qPts{1:2});
    qPtsXY = [xmat(:) ymat(:)];
    % A standard set of Z locations will be shifted around by different
    % unqQpts XY coordinates.
    zCoords = qPts{3}(:) * [0 0 1];
    qPtsXYZViaUnqIndice = @(ind)bsxfun(@plus, zCoords, [qPtsXY(ind,:) 0]);
    % From a given indice and mask, we will turn on/off the IN points under
    % that indice based on the mask. The easiest calculation is to setup
    % the IN matrix as a numZpts-by-numUnqPts mask. At the end, we must
    % unpack/reshape this 2D mask to a full 3D logical mask
    numZpts = size(zCoords,1);
    baseZinds = 1:numZpts;
    outPxIndsViaUnqIndiceMask = @(ind,mask)(ind-1)*numZpts + baseZinds(mask);
    outputSize = [numZpts, size(qPtsXY,1)];
    reshapeINfcn = @(INMASK)reshape(INMASK', cellfun(@numel, qPts([2 1 3])));
    minFacetDistanceFcn = @minFacetToQptsDistance;
end

% Start with every query point NOT inside the polyhedron. We will
% iteratively find those query points that ARE inside.
IN = false(outputSize);
% Determine with grids each query point falls into.
qPtGridXY = floor(bsxfun(@rdivide, bsxfun(@minus, qPtsXY(:,:), scalingOffsetsXY),...
    scalingRangeXY) * gridSize) + 1;
[unqQgridXY,junk,qPtGridInds] = unique(qPtGridXY,'rows');
% We need only consider grid indices within those already set up
ptsToConsidMask = ~any(qPtGridXY<1 | qPtGridXY>gridSize, 2);
if ~any(ptsToConsidMask)
    IN = reshapeINfcn(IN);
    return;
end
% Build the reference list
cellQptContents = accumarray(qPtGridInds(ptsToConsidMask),find(ptsToConsidMask), [],@(x){x});
gridsToCheck = unqQgridXY(~any(unqQgridXY<1 | unqQgridXY>gridSize, 2),:);
cellQptContents(cellfun('isempty',cellQptContents)) = [];
gridIndsToCheck = sub2ind(size(cells), gridsToCheck(:,2), gridsToCheck(:,1));

% For ease of multiplication, reshape qPt XY coords to [1-by-2-by-1-by-N]
qPtsXY = permute(qPtsXY(:,:),[4 2 3 1]);

% There will be some grid indices with query points but without facets.
emptyMask = cellfun('isempty',cells(gridIndsToCheck))';
for i = find(~emptyMask)
    % We get all the facet coordinates (ie, triangle vertices) of triangles
    % that intrude into this grid location. The size is [3-by-2-by-N], for
    % the [3vertices-by-XY-by-Ntriangles]
    allFacetInds = cells{gridIndsToCheck(i)};
    candVerts = facets(:,1:2,allFacetInds);
    % We need the XY coordinates of query points falling into this grid.
    allqPtInds = cellQptContents{i};
    queryPtsXY = qPtsXY(:,:,:,allqPtInds);

    % Get unit vectors pointing from each triangle vertex to my query point(s)
    vert2ptVecs = bsxfun(@minus, queryPtsXY, candVerts);
    vert2ptUVecs = bsxfun(@rdivide, vert2ptVecs, sqrt(sum(vert2ptVecs.^2,2)));
    % Get unit vectors pointing around each triangle (along edge A, edge B, edge C)
    edgeUVecs = allEdgeUVecs(:,:,allFacetInds);
    % Get the inner product between edgeA.edgeC, edgeB.edgeA, edgeC.edgeB
    edgeEdgeDotPs = allEdgeEdgeDotPs(:,:,allFacetInds);
    % Get inner products between each edge unit vec and the UVs from qPt to vertex
    edgeQPntDotPs = sum(bsxfun(@times, edgeUVecs, vert2ptUVecs),2);
    qPntEdgeDotPs = sum(bsxfun(@times,vert2ptUVecs, -edgeUVecs([3 1 2],:,:)),2);
    % If both inner products 2 edges to the query point are greater than the inner
    % product between the two edges themselves, the query point is between the V
    % shape made by the two edges. If this is true for all 3 edge pair, the query
    % point is inside the triangle.
    resultIN = all(bsxfun(@gt, edgeQPntDotPs, edgeEdgeDotPs) & bsxfun(@gt, qPntEdgeDotPs, edgeEdgeDotPs),1);
    resultONVERTEX = any(any(isnan(vert2ptUVecs),2),1);
    result = resultIN | resultONVERTEX;
    qPtHitsTriangles = any(result,3);
    % If NONE of the query points pierce ANY triangles, we can skip forward
    if ~any(qPtHitsTriangles), continue, end

    % In the next step, we'll need to know the indices of ALL the query points at
    % each of the distinct XY coordinates. Let's get their indices into "qPts" as a
    % cell of length M, where M is the number of unique XY points we had found.
    for ptNo = find(qPtHitsTriangles(:))'
        % Which facets does it pierce?
        piercedFacetInds = allFacetInds(result(1,1,:,ptNo));
        
        % Get the 1-by-3-by-N set of triangle normals that this qPt pierces       
        piercedTriNorms = allFacetNormals(:,:,piercedFacetInds);
        
        % Pick the first vertex as the "origin" of a plane through the facet. Get the
        % vectors from each query point to each facet origin
        facetToQptVectors = bsxfun(@minus, ...
            qPtsXYZViaUnqIndice(allqPtInds(ptNo)),...
            facets(1,:,piercedFacetInds));
        
        % Calculate how far you need to go up/down to pierce the facet's plane.
        % Positive direction means "inside" the facet, negative direction means
        % outside.
        facetToQptDists = bsxfun(@rdivide, ...
            sum(bsxfun(@times,piercedTriNorms,facetToQptVectors),2), ...
            abs(piercedTriNorms(:,3,:)));
        
        % Since it's possible for two triangles sharing the same vertex to
        % be the same distance away, I want to sum up all the distances of
        % triangles that are closest to the query point. Simple case: The
        % closest triangle is unique Edge case: The closest triangle is one
        % of many the same distance and direction away. Tricky case: The
        % closes triangle has another triangle the equivalent distance
        % but facing the opposite direction
        IN( outPxIndsViaUnqIndiceMask(allqPtInds(ptNo), ...
            minFacetDistanceFcn(facetToQptDists)<options.tol...
            )) = true;
    end
end

% If they provided X,Y,Z vectors of query points, our output is currently a
% 2D mask and must be reshaped to [LEN(Y) LEN(X) LEN(Z)].
IN = reshapeINfcn(IN);

%% Called subfunctions

% vertices = [
%     0.9046    0.1355   -0.0900
%     0.8999    0.3836   -0.0914
%     1.0572    0.2964   -0.0907
%     0.8735    0.1423   -0.1166
%     0.8685    0.4027   -0.1180
%     1.0337    0.3112   -0.1173
%     0.9358    0.1287   -0.0634
%     0.9313    0.3644   -0.0647
%     1.0808    0.2816   -0.0641
% ];
% faces = [
%      1     2     5
%      1     5     4
%      2     3     6
%      2     6     5
%      3     1     4
%      3     4     6
%      6     4     5
%      2     1     8
%      8     1     7
%      3     2     9
%      9     2     8
%      1     3     7
%      7     3     9
%      7     9     8
% ];
% point = [vertices(3,1),vertices(3,2),1.5];


function closestTriDistance = minFacetToQptDistance(facetToQptDists)
% FacetToQptDists is a 1pt-by-1-by-Nfacets array of how far you need to go
% up/down to pierce each facet's plane. If the Qpt was directly over an
% "overhang" vertex, then two facets with opposite orientation will be
% equally distant from the Qpt, with one distance positive and one
% negative. In such cases, it is impossible for the Qpt to actually be
% "inside" this pair of facets, so their distance is updated to Inf.

[junk,minInd] = min(abs(facetToQptDists),[],3);
while any( abs(facetToQptDists + facetToQptDists(minInd)) < 1e-15 )
    % Since the above comparison is made every time, but the below variable
    % setting is done only in the rare case that a query point coincides
    % with an overhang vertex, it is more efficient to re-compute the
    % equality when it's true, rather than store the result every time.
    facetToQptDists( abs(facetToQptDists) - abs(facetToQptDists(minInd)) < 1e-15) = inf;
    if ~any(isfinite(facetToQptDists))
        break;
    end
    [junk,minInd] = min(abs(facetToQptDists),[],3);
end
closestTriDistance = facetToQptDists(minInd);

function closestTriDistance = minFacetToQptsDistance(facetToQptDists)
% As above, but facetToQptDists is an Mpts-by-1-by-Nfacets array.

% The multi-point version is a little more tricky. While below is quite a
% bit slower when the while loop is entered, it is very rarely entered and
% very fast to make just the initial comparison.
[minVals,minInds] = min(abs(facetToQptDists),[],3);
while any(...
        any(abs(bsxfun(@plus,minVals,facetToQptDists))<1e-15,3) & ...
        any(abs(bsxfun(@minus,minVals,facetToQptDists))<1e-15,3))
    maskP = abs(bsxfun(@plus,minVals,facetToQptDists))<1e-15;
    maskN = abs(bsxfun(@minus,minVals,facetToQptDists))<1e-15;
    mustAlterMask = any(maskP,3) & any(maskN,3);
    for i = find(mustAlterMask)'
        facetToQptDists(i,:,maskP(i,:,:) | maskN(i,:,:)) = inf;
    end
    [newMv,newMinInds] = min(abs(facetToQptDists(mustAlterMask,:,:)),[],3);
    minInds(mustAlterMask) = newMinInds(:);
    minVals(mustAlterMask) = newMv(:);
end
% Below is a tiny speedup on basically a sub2ind call.
closestTriDistance = facetToQptDists((minInds-1)*size(facetToQptDists,1) + (1:size(facetToQptDists,1))');

%% Input handling subfunctions
function [facets, qPts, options] = parseInputs(varargin)
    
% Gather FACES and VERTICES
if isstruct(varargin{1})                        % inpolyhedron(FVstruct, ...)
    if ~all(isfield(varargin{1},{'vertices','faces'}))
        error( 'Structure FV must have "faces" and "vertices" fields' );
    end
    faces = varargin{1}.faces;
    vertices = varargin{1}.vertices;
    varargin(1) = []; % Chomp off the faces/vertices
    
else                                            % inpolyhedron(FACES, VERTICES, ...)
    faces = varargin{1};
    vertices = varargin{2};
    varargin(1:2) = []; % Chomp off the faces/vertices
end

% Unpack the faces/vertices into [3-by-3-by-N] facets. It's better to
% perform this now and have FACETS only in memory in the main program,
% rather than FACETS, FACES and VERTICES
facets = vertices';
facets = permute(reshape(facets(:,faces'), 3, 3, []),[2 1 3]);

% Extract query points
if length(varargin)<2 || ischar(varargin{2})    % inpolyhedron(F, V, [x(:) y(:) z(:)], ...)
    qPts = varargin{1};
    varargin(1) = []; % Chomp off the query points
else                                            % inpolyhedron(F, V, xVec, yVec, zVec, ...)
    qPts = varargin(1:3);
    % Chomp off the query points and tell the world that it's gridded input.
    varargin(1:3) = [];
    varargin = [varargin {'griddedInput',true}];
end
    
% Extract configurable options
options = parseOptions(varargin{:});

% Check if face normals are unified
if options.testNormals
    options.normalsAreUnified = checkNormalUnification(faces);
end

function options = parseOptions(varargin)
IP = inputParser;
IP.addParamValue('gridsize',[], @(x)isscalar(x) && isnumeric(x))
IP.addParamValue('tol', 0, @(x)isscalar(x) && isnumeric(x))
IP.addParamValue('tol_ang', 1e-5, @(x)isscalar(x) && isnumeric(x))
IP.addParamValue('facenormals',[]);
IP.addParamValue('flipnormals',false);
IP.addParamValue('griddedInput',false);
IP.addParamValue('testNormals',false);
IP.parse(varargin{:});
options = IP.Results;