function [ClosestPoint,...
Distance,...
MinimumSubsetIndices, ...
UsedCoefficients, ...
ErrorFlag] = ClosestPointInSimplex(...
SimplexVertices,...
Point)
% Purpose Find the point on a simplex which is closest to a given point.
%
% Description This routine finds the point on a simplex that is closest to an input target
% point. The routine can work in an arbitrary number n of dimensions; the dimension
% is inferred from the size of the input data. The simplex is defined as the
% convex hull of a set of input vertices. While a simplex is often thought of as
% being fully dimensional, it can also have smaller dimension than the ambient
% space. For example, it could be a line segment or a triangle in R^3.
% Since a simplex is convex (and closed),
% it contains a unique point that is closest to the input point. The closest
% point is found by Johnson s sub-distance algorithm ([Rabbitz1994]).
%
% In addition to finding the closest point, Johnson s algorithm also finds the
% smallest subsimplex that contains the nearest point. For example, the closest
% point on a tetrahedron in 3-space might actually be on an edge of the
% tetrahedron. In that case, the smallest subsimplex would be that edge, which
% is generated by its two terminal vertices. The indices of those vertices are
% returned. The closest point is a unique linear combination of those vertices,
% so the coefficients of that combination are returned. Coefficients of other
% vertices are automatically 0, so they are not returned.
%
% This routine follows [Rabbitz1994]. That reference mentions two possible
% implementations of Johnson s algorithm. One, that is followed in this routine,
% uses an exhaustive search over all subsimplices, solving a linear system for
% each subsimplex. The second implementation uses only cofactors of linear
% systems, and avoids repeated calculations, so it is more efficient than the
% first. The first implementation was chosen here because it is simpler, and
% because execution time was not a problem. This code is open source, so
% programmers who are concerned about execution time are free to modify it.
%
% Each subsimplex generates an affine hull. The closest point to Point on that
% affine hull is calculated by a linear system involving Lagrange multipliers
% (see [Rabbitz1994]). The closest point on the affine hull, however, might
% be outside the convex hull, and thus outside the simplex. In that case,
% though, the closest point on the convex hull will occur on a boundary, and
% that part of the boundary will be described by a smaller subsimplex. Since
% all subsimplices are evaluated, the closest point will eventually be found.
%
% Since this source is open source, any suggestions or improvements that reduce
% such instability are welcome.
%
% [Rabbitz1994] Rich Rabbitz, Fast Collision Detection of Moving Convex Polyhedra,
% in Section I.8 of Graphics Gems IV (IBM Version), ed. Paul Heckbert,
% Academic Press, 1994.
%
% SimplexVertices A set of up to n+1 affinely independent vectors in n dimensions.
% Their convex hull forms a simplex. The vertices are expressed
% as row vectors, stacked up to form the matrix SimplexVertices.
% The number of rows is the number of vertices, and
% the number of colums is the dimension of the ambient space.
%
% Point A point in n dimensions. Point is a row vector with n entries.
%
% ClosestPoint The closest point on the simplex to Point. ClosestPoint is a
% row vector with n entries.
%
% Distance The distance between Point and the closest point on the simplex.
%
% MinimumSubsetIndices The indices of the vertices that generate the smallest
% subsimplex that contains the closest point. The indices refer
% to the rows in the input SimplexVertices matrix.
%
% UsedCoefficients The coefficients of the linear combination of the minimal set
% of vertices that produces the closest point. The coefficients
% of vertices that are not in the minimal set is automatically 0,
% so they are not included here. The ith entry of UsedCoefficients
% is the coefficient of the ith vertex defined by MinimumSubsetIndices.
%
% ErrorFlag A Boolean variable that is true if the routine cannot return a
% solution, and false otherwise.
%
% Author Paul Centore (January 9, 2015)
%
% Copyright 2015 Paul Centore
%
% This file is part of MunsellAndKubelkaMunkToolbox.
%
% MunsellAndKubelkaMunkToolbox is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% MunsellAndKubelkaMunkToolbox is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with MunsellAndKubelkaMunkToolbox. If not, see .
% Initialize variables
ClosestPoint = [] ;
Distance = -99 ;
MinimumSubsetIndices = [] ;
UsedCoefficients = [] ;
ErrorFlag = false ;
% Minimum distance for comparisons, to avoid numerical roundoff error
EpsDist = 1e-14 ;
% Infer the number of vertices and the dimension of the ambient space from the size of the
% input variables
[NumberOfVertices, DimensionOfSpace] = size(SimplexVertices) ;
% Translate the input point to the origin; translate the simplex by the same amount. This
% translation will simplify calculations
TranslatedSimplexVertices = SimplexVertices - repmat(Point,NumberOfVertices,1) ;
% For diagnostic information, if interested
if false
Pairs = combinator(NumberOfVertices, 2, 'c') ;
NumberOfPairs = size(Pairs,1) ;
DistancesBetweenVertices = [] ;
for ctr = 1:NumberOfPairs
DistanceBetweenVertices = norm(SimplexVertices(Pairs(ctr,1),:) - SimplexVertices(Pairs(ctr,2),:)) ;
DistancesBetweenVertices = [DistancesBetweenVertices, DistanceBetweenVertices] ;
end
%DistancesBetweenVertices
DistancesToVertices = [] ;
for ctr = 1:NumberOfVertices
DistanceToVertex = norm(TranslatedSimplexVertices(ctr,:)) ;
DistancesToVertices = [DistancesToVertices, DistanceToVertex] ;
end
%DistancesToVertices
end
% Any subset of vertices will generate a subsimplex, and every subsimplex is generated by
% a subset of vertices. To find the set of all subsimplices, find the power set of all
% the vertices.
IndicesOfVertices = 1:NumberOfVertices ;
PowerSetOfIndices = nchoose(IndicesOfVertices) ;
% This implementation loops exhaustively through the set of subsimplices. For each
% subsimplex, the algorithm finds the closest point to Point on the affine hull of that
% subsimplex. That closest point is expressed as a linear combination of the subsimplex s
% vertices, whose coefficients are given in a vector denoted lambda. If all the entries
% in lambda are between 0 and 1, then the closest point on the affine hull is also in the
% convex hull, and is therefore a candidate for the closest point on the whole simplex (which
% is the convex hull of its generating vertices).
% The following loop produces three matrices. The first matrix gives the closest point on
% the affine hull for each subsimplex; each row of the matrix is a point in n-space, and there
% is a row for each susbsimplex. The second matrix is a column vector of distances from Point
% to that subsimplex; an entry is NaN if the closest point on the affine hull is not contained
% in the convex hull of the subsimplex. The third matrix gives the coefficients of the
% generating vertices, that lead to the closest point in the affine hull. Each row corresponds
% to one subsimplex; each column corresponds to one generating vector. If a particular
% generating vector does not appear in a particular simplex, then the entry in lambdas is NaN.
% A point is in the convex hull of the subsimplex if and only if all its entries are between
% 0 and 1.
ClosestPointsInSubSimplices = NaN * ones(length(PowerSetOfIndices),DimensionOfSpace) ;
MinDistancesToSubSimplices = NaN * ones(length(PowerSetOfIndices),1) ;
lambdas = NaN * ones(length(PowerSetOfIndices),NumberOfVertices) ;
for SubSimplexCtr = 1:length(PowerSetOfIndices)
% Extract the vertices for a particular subsimplex.
SubSimplexIndices = PowerSetOfIndices{SubSimplexCtr} ;
NumberOfSubSimplexIndices = length(SubSimplexIndices) ;
% The Johnson Sub-Distance Algorithm defines a linear system A x lambda = b. The
% vector lambda gives coefficients in the linear combination of the subsimplex s
% vertices. The point defined by these coefficients is the closest point to the origin,
% on the affine hull of that subsimplex. See [Rabbitz1994] for details.
% Construct the matrix A in the system A x lambda = b.
JohnsonMatrixA = NaN * ones(NumberOfSubSimplexIndices,NumberOfSubSimplexIndices) ;
JohnsonMatrixA(1,:) = ones(1,NumberOfSubSimplexIndices) ;
for rowctr = 2:NumberOfSubSimplexIndices
for colctr = 1:NumberOfSubSimplexIndices
JohnsonMatrixA(rowctr,colctr) = dot( ...
(TranslatedSimplexVertices(SubSimplexIndices(rowctr),:) - ...
TranslatedSimplexVertices(SubSimplexIndices(1),:)), ...
TranslatedSimplexVertices(SubSimplexIndices(colctr),:)) ;
end
end
% Construct the vector b in the system A x lambda = b.
JohnsonVectorb = [1; zeros(NumberOfSubSimplexIndices-1,1)] ;
% Numerical instability will result if the matrix A is ill-conditioned. The top row
% of A and the first entry of b are all ones, to insure that the lambdas sum to 1.
% Multiplying the top row of A and the first entry of b by the same constant should
% not change the system solution. Choose a constant that increases the condition
% number (though at the expense of increasing the determinant).
% This method of handling ill-conditioning is awkward and not always effective; more
% elegant and effective handling methods would be welcome.
RcondThreshold = 1e-5 ;
if rcond(JohnsonMatrixA) < RcondThreshold
% disp(['Ill-conditioned matrix in ClosestPointInSimplex: adjusting'])
% SubSimplexIndices
% JohnsonMatrixA
JohnsonMatrixA(1,:) = (1/RcondThreshold) * JohnsonMatrixA(1,:) ;
JohnsonVectorb(1) = (1/RcondThreshold) ;
end
% Solve the system for lambda, if the condition number is not too low
if rcond(JohnsonMatrixA) > 1e-15
lambda = JohnsonMatrixA \ JohnsonVectorb ;
else
if false % Use this code to output information for analysis
disp(['ERROR in routine ClosestPointInSimplex: ill-conditioned matrix:'])
JohnsonMatrixA
RConditionNumber = rcond(JohnsonMatrixA)
ranktest = rank(SimplexVertices(reshape(SubSimplexIndices,length(SubSimplexIndices),1),:)) ;
lengthrank = [length(SubSimplexIndices),ranktest]
end
ErrorFlag = true ;
return
end
% Save the lambda values for this subsimplex in the matrix lambdas. Leave an entry
% in lambdas as NaN if a particular generating vector is not contained in the
% current subsimplex.
for ctr = 1:length(lambda)
lambdas(SubSimplexCtr,SubSimplexIndices(ctr)) = lambda(ctr) ;
end
% The closest point in the affine hull of the subsimplex is the linear combination
% of that subsimplex s vertices, using the coefficients in lambda
ClosestPointInSubSimplex = zeros(1,DimensionOfSpace) ;
for ctr = 1:length(lambda)
ClosestPointInSubSimplex = ClosestPointInSubSimplex + ...
(lambda(ctr) * TranslatedSimplexVertices(SubSimplexIndices(ctr),:)) ;
end
% The closest point is in the convex hull of the subsimplex if and only if all the
% coefficients in lambda are between 0 and 1. Check this condition, and switch the
% closest point to NaNs if it is not satisfied.
if min(min(lambda)) < 0 || max(max(lambda)) > 1
ClosestPointInSubSimplex = NaN * ones(1,DimensionOfSpace) ;
end
ClosestPointsInSubSimplices(SubSimplexCtr,:) = ClosestPointInSubSimplex ;
% Record the minimum distance from the origin (the translation of Point) to the
% subsimplex, using an NaN if the closest point is not in the convex hull of the
% subsimplex s vertices
MinDistancesToSubSimplices(SubSimplexCtr) = norm(ClosestPointInSubSimplex) ;
end
% We are interested in the point on the simplex that is nearest to Point, so sort the
% distances from smallest to greatest.
[SortedDistances, SortedIndices] = sort(MinDistancesToSubSimplices) ;
% Numerical issues might cause multiple subsimplices, with different numbers of vertices,
% both to return the same minimum distance point. In that case, select the subsimplex
% with the smallest number of vertices. The following loop identifies all points, on any
% subsimplices, which give the minimum distance.
BestIndex = SortedIndices(1) ;
BestDistance = SortedDistances(1) ;
NumberOfVertices = length(PowerSetOfIndices{BestIndex}) ;
SigDifFound = false ;
SubCtr = 2 ;
while ~SigDifFound && SubCtr <= size(MinDistancesToSubSimplices,1)
NewIndex = SortedIndices(SubCtr) ;
NewDistance = SortedDistances(SubCtr) ;
NewNumberOfVertices = length(PowerSetOfIndices{SortedIndices(SubCtr)}) ;
% Check if the next distance is identical (to within negligible numerical error) to
% the best distance. If it is, then check whether the new distance comes from a
% subsimplex with smaller vertices. Update it if it does.
if abs(NewDistance - BestDistance) < EpsDist
if NewNumberOfVertices < NumberOfVertices ;
BestIndex = NewIndex ;
NumberOfVertices = NewNumberOfVertices ;
end
else
% The distances have been sorted, so once one distance is larger than the minimum
% distance, the loop can terminate.
SigDifFound = true ;
end
SubCtr = SubCtr + 1 ;
end
% The point in the smallest subsimplex is returned, along with the indices of that
% subsimplex. The closest point is now translated back.
MinimumSubsetIndices = PowerSetOfIndices{BestIndex} ;
ClosestPoint = ClosestPointsInSubSimplices(BestIndex,:) + Point ;
Distance = norm(ClosestPoint - Point) ;
% The lambdas are the coefficients of the generating vertices. Return them to provide
% more information to the user. If any lambda is NaN, then it is automatically 0,
% so it is not returned.
Coefficients = lambdas(BestIndex,:) ;
UsedCoefficients = [] ;
for ctr = 1:length(Coefficients)
if ~isnan(Coefficients(ctr))
UsedCoefficients = [UsedCoefficients, Coefficients(ctr)] ;
end
end