function in = intriangulation(vertices,faces,testp,heavytest)
% intriangulation: Test points in 3d wether inside or outside a (closed) triangulation
% usage: in = intriangulation(vertices,faces,testp,heavytest)
%
% arguments: (input)
%  vertices   - points in 3d as matrix with three columns
%
%  faces      - description of triangles as matrix with three columns.
%               Each row contains three indices into the matrix of vertices
%               which gives the three cornerpoints of the triangle.
%
%  testp      - points in 3d as matrix with three columns
%
%  heavytest  - int n >= 0. Perform n additional randomized rotation tests.
%
% IMPORTANT: the set of vertices and faces has to form a watertight surface!
%
% arguments: (output)
%  in - a vector of length size(testp,1), containing 0 and 1.
%       in(nr) =  0: testp(nr,:) is outside the triangulation
%       in(nr) =  1: testp(nr,:) is inside the triangulation
%       in(nr) = -1: unable to decide for testp(nr,:) 
%
% Thanks to Adam A for providing the FEX submission voxelise. The
% algorithms of voxelise form the algorithmic kernel of intriangulation.
%
% Thanks to Sven to discussions about speed and avoiding problems in
% special cases.
%
% Example usage:
%
%      n = 10;
%      vertices = rand(n, 3)-0.5; % Generate random points
%      tetra = delaunayn(vertices); % Generate delaunay triangulization
%      faces = freeBoundary(TriRep(tetra,vertices)); % use free boundary as triangulation
%      n = 1000;
%      testp = 2*rand(n,3)-1; % Generate random testpoints
%      in = intriangulation(vertices,faces,testp);
%      % Plot results
%      h = trisurf(faces,vertices(:,1),vertices(:,2),vertices(:,3));
%      set(h,'FaceColor','black','FaceAlpha',1/3,'EdgeColor','none');
%      hold on;
%      plot3(testp(:,1),testp(:,2),testp(:,3),'b.');
%      plot3(testp(in==1,1),testp(in==1,2),testp(in==1,3),'ro');
%
% See also: intetrahedron, tsearchn, inpolygon
%
% Author: Johannes Korsawe, heavily based on voxelise from Adam A.
% E-mail: johannes.korsawe@volkswagen.de
% Release: 1.3
% Release date: 25/09/2013

% check number of inputs
if nargin<3,
    fprintf('??? Error using ==> intriangulation\nThree input matrices are needed.\n');in=[];return;
end
if nargin==3,
    heavytest = 0;
end
% check size of inputs
if size(vertices,2)~=3 || size(faces,2)~=3 || size(testp,2)~=3,
    fprintf('??? Error using ==> intriagulation\nAll input matrices must have three columns.\n');in=[];return;
end
ipmax = max(faces(:));zerofound = ~isempty(find(faces(:)==0, 1));
if ipmax>size(vertices,1) || zerofound,
    fprintf('??? Error using ==> intriangulation\nThe triangulation data is defect. use trisurf(faces,vertices(:,1),vertices(:,2),vertices(:,3)) for test of deficiency.\n');return;
end

% loop for heavytest
inreturn = zeros(size(testp,1),1);VER = vertices;TESTP = testp;

for n = 1:heavytest+1,

    % Randomize
    if n>1,
        v=rand(1,3);D=rotmatrix(v/norm(v),rand*180/pi);vertices=VER*D;testp = TESTP*D;
    else,
        vertices=VER;
    end
    
    % Preprocessing data
    meshXYZ = zeros(size(faces,1),3,3);
    for loop = 1:3,
        meshXYZ(:,:,loop) = vertices(faces(:,loop),:);
    end

    % Basic idea (ingenious from FeX-submission voxelise):
    % If point is inside, it will cross the triangulation an uneven number of times in each direction (x, -x, y, -y, z, -z).
    
    % The function VOXELISEinternal is about 98% identical to its version inside voxelise.m.
    % This includes the elaborate comments. Thanks to Adam A!
    
    % z-direction:
    % intialization of results and correction list
    [in,cl] = VOXELISEinternal(testp(:,1),testp(:,2),testp(:,3),meshXYZ);
    
    % x-direction:
    % has only to be done for those points, that were not determinable in the first step --> cl
    [in2,cl2] = VOXELISEinternal(testp(cl,2),testp(cl,3),testp(cl,1),meshXYZ(:,[2,3,1],:));
    % Use results of x-direction that determined "inside"
    in(cl(in2==1)) = 1;
    % remaining indices with unclear result
    cl = cl(cl2);
    
    % y-direction:
    % has only to be done for those points, that were not determinable in the first and second step --> cl
    [in3,cl3] = VOXELISEinternal(testp(cl,3),testp(cl,1),testp(cl,2),meshXYZ(:,[3,1,2],:));
    
    % Use results of y-direction that determined "inside"
    in(cl(in3==1)) = 1;
    % remaining indices with unclear result
    cl = cl(cl3);
    
    % mark those indices, where all three tests have failed
    in(cl) = -1;
    
    if n==1,
        inreturn = in;  % Starting guess
    else,
        % if ALWAYS inside, use as inside!
%        I = find(inreturn ~= in);
%        inreturn(I(in(I)==0)) = 0;
        
        % if AT LEAST ONCE inside, use as inside!
        I = find(inreturn ~= in);
        inreturn(I(in(I)==1)) = 1;
        
    end

end

in = inreturn;

end

%==========================================================================
function [OUTPUT,correctionLIST] = VOXELISEinternal(testx,testy,testz,meshXYZ)

% Prepare logical array to hold the logical data:
OUTPUT = false(size(testx,1),1);

%Identify the min and max x,y coordinates of the mesh:
meshZmin = min(min(meshXYZ(:,3,:)));meshZmax = max(max(meshXYZ(:,3,:)));

%Identify the min and max x,y,z coordinates of each facet:
meshXYZmin = min(meshXYZ,[],3);meshXYZmax = max(meshXYZ,[],3);

%======================================================
% TURN OFF DIVIDE-BY-ZERO WARNINGS
%======================================================
%This prevents the Y1predicted, Y2predicted, Y3predicted and YRpredicted
%calculations creating divide-by-zero warnings.  Suppressing these warnings
%doesn't affect the code, because only the sign of the result is important.
%That is, 'Inf' and '-Inf' results are ok.
%The warning will be returned to its original state at the end of the code.
warningrestorestate = warning('query', 'MATLAB:divideByZero');
%warning off MATLAB:divideByZero

%======================================================
% START COMPUTATION
%======================================================

correctionLIST = [];   %Prepare to record all rays that fail the voxelisation.  This array is built on-the-fly, but since
%it ought to be relatively small should not incur too much of a speed penalty.

% Loop through each testpoint.
% The testpoint-array will be tested by passing rays in the z-direction through
% each x,y coordinate of the testpoints, and finding the locations where the rays cross the mesh.
facetCROSSLIST = zeros(1,1e3);  % uses countindex: nf
nm = size(meshXYZmin,1);
for loop = 1:length(OUTPUT),
    
    nf = 0;
%    % - 1a - Find which mesh facets could possibly be crossed by the ray:
%    possibleCROSSLISTy = find( meshXYZmin(:,2)<=testy(loop) & meshXYZmax(:,2)>=testy(loop) );

%    % - 1b - Find which mesh facets could possibly be crossed by the ray:
%    possibleCROSSLIST = possibleCROSSLISTy( meshXYZmin(possibleCROSSLISTy,1)<=testx(loop) & meshXYZmax(possibleCROSSLISTy,1)>=testx(loop) );

    % Do - 1a - and - 1b - faster
    possibleCROSSLISTy = find((testy(loop)-meshXYZmin(:,2)).*(meshXYZmax(:,2)-testy(loop))>0);
    possibleCROSSLISTx = (testx(loop)-meshXYZmin(possibleCROSSLISTy,1)).*(meshXYZmax(possibleCROSSLISTy,1)-testx(loop))>0;
    possibleCROSSLIST = possibleCROSSLISTy(possibleCROSSLISTx);
    
    if isempty(possibleCROSSLIST)==0  %Only continue the analysis if some nearby facets were actually identified
        
        % - 2 - For each facet, check if the ray really does cross the facet rather than just passing it close-by:
        
        % GENERAL METHOD:
        % 1. Take each edge of the facet in turn.
        % 2. Find the position of the opposing vertex to that edge.
        % 3. Find the position of the ray relative to that edge.
        % 4. Check if ray is on the same side of the edge as the opposing vertex.
        % 5. If this is true for all three edges, then the ray definitely passes through the facet.
        %
        % NOTES:
        % 1. If the ray crosses exactly on an edge, this is counted as crossing the facet.
        % 2. If a ray crosses exactly on a vertex, this is also taken into account.
        
        for loopCHECKFACET = possibleCROSSLIST'
            
            %Check if ray crosses the facet.  This method is much (>>10 times) faster than using the built-in function 'inpolygon'.
            %Taking each edge of the facet in turn, check if the ray is on the same side as the opposing vertex.  If so, let testVn=1
            
            Y1predicted = meshXYZ(loopCHECKFACET,2,2) - ((meshXYZ(loopCHECKFACET,2,2)-meshXYZ(loopCHECKFACET,2,3)) * (meshXYZ(loopCHECKFACET,1,2)-meshXYZ(loopCHECKFACET,1,1))/(meshXYZ(loopCHECKFACET,1,2)-meshXYZ(loopCHECKFACET,1,3)));
            YRpredicted = meshXYZ(loopCHECKFACET,2,2) - ((meshXYZ(loopCHECKFACET,2,2)-meshXYZ(loopCHECKFACET,2,3)) * (meshXYZ(loopCHECKFACET,1,2)-testx(loop))/(meshXYZ(loopCHECKFACET,1,2)-meshXYZ(loopCHECKFACET,1,3)));
            
            if (Y1predicted > meshXYZ(loopCHECKFACET,2,1) && YRpredicted > testy(loop)) || (Y1predicted < meshXYZ(loopCHECKFACET,2,1) && YRpredicted < testy(loop)) || (meshXYZ(loopCHECKFACET,2,2)-meshXYZ(loopCHECKFACET,2,3)) * (meshXYZ(loopCHECKFACET,1,2)-testx(loop)) == 0
%                testV1 = 1;   %The ray is on the same side of the 2-3 edge as the 1st vertex.
            else
%                testV1 = 0;   %The ray is on the opposite side of the 2-3 edge to the 1st vertex.
                % As the check is for ALL three checks to be true, we can continue here, if only one check fails
                continue;
            end %if
            
            Y2predicted = meshXYZ(loopCHECKFACET,2,3) - ((meshXYZ(loopCHECKFACET,2,3)-meshXYZ(loopCHECKFACET,2,1)) * (meshXYZ(loopCHECKFACET,1,3)-meshXYZ(loopCHECKFACET,1,2))/(meshXYZ(loopCHECKFACET,1,3)-meshXYZ(loopCHECKFACET,1,1)));
            YRpredicted = meshXYZ(loopCHECKFACET,2,3) - ((meshXYZ(loopCHECKFACET,2,3)-meshXYZ(loopCHECKFACET,2,1)) * (meshXYZ(loopCHECKFACET,1,3)-testx(loop))/(meshXYZ(loopCHECKFACET,1,3)-meshXYZ(loopCHECKFACET,1,1)));
            if (Y2predicted > meshXYZ(loopCHECKFACET,2,2) && YRpredicted > testy(loop)) || (Y2predicted < meshXYZ(loopCHECKFACET,2,2) && YRpredicted < testy(loop)) || (meshXYZ(loopCHECKFACET,2,3)-meshXYZ(loopCHECKFACET,2,1)) * (meshXYZ(loopCHECKFACET,1,3)-testx(loop)) == 0
%                testV2 = 1;   %The ray is on the same side of the 3-1 edge as the 2nd vertex.
            else
%                testV2 = 0;   %The ray is on the opposite side of the 3-1 edge to the 2nd vertex.
                % As the check is for ALL three checks to be true, we can continue here, if only one check fails
                continue;
            end %if            
            
            Y3predicted = meshXYZ(loopCHECKFACET,2,1) - ((meshXYZ(loopCHECKFACET,2,1)-meshXYZ(loopCHECKFACET,2,2)) * (meshXYZ(loopCHECKFACET,1,1)-meshXYZ(loopCHECKFACET,1,3))/(meshXYZ(loopCHECKFACET,1,1)-meshXYZ(loopCHECKFACET,1,2)));
            YRpredicted = meshXYZ(loopCHECKFACET,2,1) - ((meshXYZ(loopCHECKFACET,2,1)-meshXYZ(loopCHECKFACET,2,2)) * (meshXYZ(loopCHECKFACET,1,1)-testx(loop))/(meshXYZ(loopCHECKFACET,1,1)-meshXYZ(loopCHECKFACET,1,2)));
            if (Y3predicted > meshXYZ(loopCHECKFACET,2,3) && YRpredicted > testy(loop)) || (Y3predicted < meshXYZ(loopCHECKFACET,2,3) && YRpredicted < testy(loop)) || (meshXYZ(loopCHECKFACET,2,1)-meshXYZ(loopCHECKFACET,2,2)) * (meshXYZ(loopCHECKFACET,1,1)-testx(loop)) == 0
%                testV3 = 1;   %The ray is on the same side of the 1-2 edge as the 3rd vertex.
            else
%                testV3 = 0;   %The ray is on the opposite side of the 1-2 edge to the 3rd vertex.
                % As the check is for ALL three checks to be true, we can continue here, if only one check fails
                continue;
            end %if
    
            nf=nf+1;facetCROSSLIST(nf)=loopCHECKFACET;
            
        end %for

        % Use only values ~=0
        facetCROSSLIST = facetCROSSLIST(1:nf);
        
        % - 3 - Find the z coordinate of the locations where the ray crosses each facet:
        gridCOzCROSS = zeros(1,nf);
        for loopFINDZ = facetCROSSLIST
            
            % METHOD:
            % 1. Define the equation describing the plane of the facet.  For a
            % more detailed outline of the maths, see:
            % http://local.wasp.uwa.edu.au/~pbourke/geometry/planeeq/
            %    Ax + By + Cz + D = 0
            %    where  A = y1 (z2 - z3) + y2 (z3 - z1) + y3 (z1 - z2)
            %           B = z1 (x2 - x3) + z2 (x3 - x1) + z3 (x1 - x2)
            %           C = x1 (y2 - y3) + x2 (y3 - y1) + x3 (y1 - y2)
            %           D = - x1 (y2 z3 - y3 z2) - x2 (y3 z1 - y1 z3) - x3 (y1 z2 - y2 z1)
            % 2. For the x and y coordinates of the ray, solve these equations to find the z coordinate in this plane.
            
            planecoA = meshXYZ(loopFINDZ,2,1)*(meshXYZ(loopFINDZ,3,2)-meshXYZ(loopFINDZ,3,3)) + meshXYZ(loopFINDZ,2,2)*(meshXYZ(loopFINDZ,3,3)-meshXYZ(loopFINDZ,3,1)) + meshXYZ(loopFINDZ,2,3)*(meshXYZ(loopFINDZ,3,1)-meshXYZ(loopFINDZ,3,2));
            planecoB = meshXYZ(loopFINDZ,3,1)*(meshXYZ(loopFINDZ,1,2)-meshXYZ(loopFINDZ,1,3)) + meshXYZ(loopFINDZ,3,2)*(meshXYZ(loopFINDZ,1,3)-meshXYZ(loopFINDZ,1,1)) + meshXYZ(loopFINDZ,3,3)*(meshXYZ(loopFINDZ,1,1)-meshXYZ(loopFINDZ,1,2));
            planecoC = meshXYZ(loopFINDZ,1,1)*(meshXYZ(loopFINDZ,2,2)-meshXYZ(loopFINDZ,2,3)) + meshXYZ(loopFINDZ,1,2)*(meshXYZ(loopFINDZ,2,3)-meshXYZ(loopFINDZ,2,1)) + meshXYZ(loopFINDZ,1,3)*(meshXYZ(loopFINDZ,2,1)-meshXYZ(loopFINDZ,2,2));
            planecoD = - meshXYZ(loopFINDZ,1,1)*(meshXYZ(loopFINDZ,2,2)*meshXYZ(loopFINDZ,3,3)-meshXYZ(loopFINDZ,2,3)*meshXYZ(loopFINDZ,3,2)) - meshXYZ(loopFINDZ,1,2)*(meshXYZ(loopFINDZ,2,3)*meshXYZ(loopFINDZ,3,1)-meshXYZ(loopFINDZ,2,1)*meshXYZ(loopFINDZ,3,3)) - meshXYZ(loopFINDZ,1,3)*(meshXYZ(loopFINDZ,2,1)*meshXYZ(loopFINDZ,3,2)-meshXYZ(loopFINDZ,2,2)*meshXYZ(loopFINDZ,3,1));
            
            if abs(planecoC) < 1e-14
                planecoC=0;
            end
            
            gridCOzCROSS(facetCROSSLIST==loopFINDZ) = (- planecoD - planecoA*testx(loop) - planecoB*testy(loop)) / planecoC;
            
        end %for

        if isempty(gridCOzCROSS),continue;end
        
        %Remove values of gridCOzCROSS which are outside of the mesh limits (including a 1e-12 margin for error).
        gridCOzCROSS = gridCOzCROSS( gridCOzCROSS>=meshZmin-1e-12 & gridCOzCROSS<=meshZmax+1e-12 );

        %Round gridCOzCROSS to remove any rounding errors, and take only the unique values:
        gridCOzCROSS = round(gridCOzCROSS*1e10)/1e10;
        
        % Replacement of the call to unique (gridCOzCROSS = unique(gridCOzCROSS);) by the following line:
        tmp = sort(gridCOzCROSS);I=[0,tmp(2:end)-tmp(1:end-1)]~=0;gridCOzCROSS = [tmp(1),tmp(I)];
        
        % - 4 - Label as being inside the mesh all the voxels that the ray passes through after crossing one facet before crossing another facet:
        
        if rem(numel(gridCOzCROSS),2)==0  % Only rays which cross an even number of facets are voxelised
            
            for loopASSIGN = 1:(numel(gridCOzCROSS)/2)
                voxelsINSIDE = (testz(loop)>gridCOzCROSS(2*loopASSIGN-1) & testz(loop)<gridCOzCROSS(2*loopASSIGN));
                OUTPUT(loop) = voxelsINSIDE;
                if voxelsINSIDE,break;end
            end %for

            
        elseif numel(gridCOzCROSS)~=0    % Remaining rays which meet the mesh in some way are not voxelised, but are labelled for correction later.
            correctionLIST = [ correctionLIST; loop ];
        end %if
        
    end %if
    
end %for

%======================================================
% RESTORE DIVIDE-BY-ZERO WARNINGS TO THE ORIGINAL STATE
%======================================================

warning(warningrestorestate)

% J.Korsawe: A correction is not possible as the testpoints need not to be
%            ordered in any way.
%            voxelise contains a correction algorithm which is appended here
%            without changes in syntax.
return

%======================================================
% USE INTERPOLATION TO FILL IN THE RAYS WHICH COULD NOT BE VOXELISED
%======================================================
%For rays where the voxelisation did not give a clear result, the ray is
%computed by interpolating from the surrounding rays.
countCORRECTIONLIST = size(correctionLIST,1);

if countCORRECTIONLIST>0
    
  %If necessary, add a one-pixel border around the x and y edges of the
  %array.  This prevents an error if the code tries to interpolate a ray at
  %the edge of the x,y grid.
  if min(correctionLIST(:,1))==1 || max(correctionLIST(:,1))==numel(gridCOx) || min(correctionLIST(:,2))==1 || max(correctionLIST(:,2))==numel(gridCOy)
    gridOUTPUT     = [zeros(1,voxcountY+2,voxcountZ);zeros(voxcountX,1,voxcountZ),gridOUTPUT,zeros(voxcountX,1,voxcountZ);zeros(1,voxcountY+2,voxcountZ)];
    correctionLIST = correctionLIST + 1;
  end
  
  for loopC = 1:countCORRECTIONLIST
    voxelsforcorrection = squeeze( sum( [ gridOUTPUT(correctionLIST(loopC,1)-1,correctionLIST(loopC,2)-1,:) ,...
                                          gridOUTPUT(correctionLIST(loopC,1)-1,correctionLIST(loopC,2),:)   ,...
                                          gridOUTPUT(correctionLIST(loopC,1)-1,correctionLIST(loopC,2)+1,:) ,...
                                          gridOUTPUT(correctionLIST(loopC,1),correctionLIST(loopC,2)-1,:)   ,...
                                          gridOUTPUT(correctionLIST(loopC,1),correctionLIST(loopC,2)+1,:)   ,...
                                          gridOUTPUT(correctionLIST(loopC,1)+1,correctionLIST(loopC,2)-1,:) ,...
                                          gridOUTPUT(correctionLIST(loopC,1)+1,correctionLIST(loopC,2),:)   ,...
                                          gridOUTPUT(correctionLIST(loopC,1)+1,correctionLIST(loopC,2)+1,:) ,...
                                         ] ) );
    voxelsforcorrection = (voxelsforcorrection>=4);
    gridOUTPUT(correctionLIST(loopC,1),correctionLIST(loopC,2),voxelsforcorrection) = 1;
  end %for

  %Remove the one-pixel border surrounding the array, if this was added
  %previously.
  if size(gridOUTPUT,1)>numel(gridCOx) || size(gridOUTPUT,2)>numel(gridCOy)
    gridOUTPUT = gridOUTPUT(2:end-1,2:end-1,:);
  end
  
end %if

%disp([' Ray tracing result: ',num2str(countCORRECTIONLIST),' rays (',num2str(countCORRECTIONLIST/(voxcountX*voxcountY)*100,'%5.1f'),'% of all rays) exactly crossed a facet edge and had to be computed by interpolation.'])

end %function
%==========================================================================

function D = rotmatrix(v,deg)
% calculate the rotation matrix about v by deg degrees

deg=deg/180*pi;
if deg~=0,
    v=v/norm(v);
    v1=v(1);v2=v(2);v3=v(3);ca=cos(deg);sa=sin(deg);
    D=[ca+v1*v1*(1-ca),v1*v2*(1-ca)-v3*sa,v1*v3*(1-ca)+v2*sa;
        v2*v1*(1-ca)+v3*sa,ca+v2*v2*(1-ca),v2*v3*(1-ca)-v1*sa;
        v3*v1*(1-ca)-v2*sa,v3*v2*(1-ca)+v1*sa,ca+v3*v3*(1-ca)];
else,
    D=eye(3,3);
end

end