% Given a triangulation, the set of points from which it was created,
% and the corresponding pixel values, this function estimates the
% gradient vector of image intensity at each vertex in the triangulation.
function vGradientVecs = getGradientVecs(tri,registeredPts,pixelVals)

[totalTris,three] = size(tri);
[totalPoints,two] = size(registeredPts);

%   1. Find the unit normal vectors and the areas of all triangles,
%      then find the product of these two numbers for each triangle
triAreas = zeros(totalTris,1);
triUnitNormals = zeros(totalTris,3);
for triNum = 1:totalTris
    v = tri(triNum,:);
    
    % 3D triangle points: x,y,pixel
    p = [registeredPts(v,:),pixelVals(v)];
    
    % triangle area
    triAreas(triNum) = polyarea([p(:,1)],[p(:,2)]);
    
    % directional vectors representing the surface of the plane
    d1 = p(2,:)-p(1,:);
    d2 = p(3,:)-p(2,:);
    
    % cross product of these vectors
    crossp = cross(d1,d2);
         
    % normalized cross product = unit normal vector for the triangle
    dist = sqrt(sum(crossp.^2));
    triUnitNormals(triNum,:) = crossp./dist;
end

%   2. Estimate the unit normal vector at each vertex
%       a. Find the triangle patches that neighbor the vertex
%       b. Find the unit normal vectors of these regions
%       c. Multiply each of these vectors by the area of the 
%          associated region, then sum these numbers and divide
%          by the total area of all the regions
vUnitNormals = zeros(totalPoints,3);
for pointNum = 1:totalPoints
    [neighbors,x] = find(tri==pointNum);
    areas = triAreas(neighbors);
    areas3 = [areas,areas,areas];
    triNormsSum = sum(triUnitNormals(neighbors,:).*areas3);
    triAreasSum = sum(areas);
    
    if( triAreasSum == 0 )
        triAreasSum = 0.0001;
    vUnormalized = triNormsSum./triAreasSum;
    
    % re-normalize
    vUnitNormals(pointNum,:) = ...
        vUnormalized./sqrt(sum(vUnormalized.^2));
end

%   3. Find the gradients along the x and y directions for each vertex
%      vertex's unit normal: n = [nx,ny,nz]
%      x-direction gradient: dz/dx = -nx/nz
%      y-direction gradient: dz/dy = -ny/nz
vGradientVecs = zeros(totalPoints,2);
for pointNum = 1:totalPoints
    nz = vUnitNormals(pointNum,3);
    if( nz == 0 )
        nz = 0.0001;
    end
    vGradientVecs(pointNum,1) = -vUnitNormals(pointNum,1)./nz;
    vGradientVecs(pointNum,2) = -vUnitNormals(pointNum,2)./nz;
end

end