function [dist,PP0] = pointTriangleDistance(TRI,P)
% calculate distance between a point and a triangle in 3D
% SYNTAX
% dist = pointTriangleDistance(TRI,P)
% [dist,PP0] = pointTriangleDistance(TRI,P)
%
% DESCRIPTION
% Calculate the distance of a given point P from a triangle TRI.
% Point P is a row vector of the form 1x3. The triangle is a matrix
% formed by three rows of points TRI = [P1;P2;P3] each of size 1x3.
% dist = pointTriangleDistance(TRI,P) returns the distance of the point P
% to the triangle TRI.
% [dist,PP0] = pointTriangleDistance(TRI,P) additionally returns the
% closest point PP0 to P on the triangle TRI.
%
% Author: Gwendolyn Fischer
% Release: 1.0
% Release date: 09/02/02
% Release: 1.1 Fixed Bug because of normalization
% Release: 1.2 Fixed Bug because of typo in region 5 20101013
% Release: 1.3 Fixed Bug because of typo in region 2 20101014
% Possible extention could be a version tailored not to return the distance
% and additionally the closest point, but instead return only the closest
% point. Could lead to a small speed gain.
% Example:
% %% The Problem
% P0 = [0.5 -0.3 0.5];
%
% P1 = [0 -1 0];
% P2 = [1 0 0];
% P3 = [0 0 0];
%
% vertices = [P1; P2; P3];
% faces = [1 2 3];
%
% %% The Engine
% [dist,PP0] = pointTriangleDistance([P1;P2;P3],P0);
%
% %% Visualization
% [x,y,z] = sphere(20);
% x = dist*x+P0(1);
% y = dist*y+P0(2);
% z = dist*z+P0(3);
%
% figure
% hold all
% patch('Vertices',vertices,'Faces',faces,'FaceColor','r','FaceAlpha',0.8);
% plot3(P0(1),P0(2),P0(3),'b*');
% plot3(PP0(1),PP0(2),PP0(3),'*g')
% surf(x,y,z,'FaceColor','b','FaceAlpha',0.3)
% view(3)
% The algorithm is based on
% "David Eberly, 'Distance Between Point and Triangle in 3D',
% Geometric Tools, LLC, (1999)"
% http:\\www.geometrictools.com/Documentation/DistancePoint3Triangle3.pdf
%
% ^t
% \ |
% \reg2|
% \ |
% \ |
% \ |
% \|
% *P2
% |\
% | \
% reg3 | \ reg1
% | \
% |reg0\
% | \
% | \ P1
% -------*-------*------->s
% |P0 \
% reg4 | reg5 \ reg6
%% Do some error checking
if nargin<2
error('pointTriangleDistance: too few arguments see help.');
end
P = P(:)';
if size(P,2)~=3
error('pointTriangleDistance: P needs to be of length 3.');
end
if size(TRI)~=[3 3]
error('pointTriangleDistance: TRI needs to be of size 3x3.');
end
% ToDo: check for colinearity and/or too small triangles.
% rewrite triangle in normal form
B = TRI(1,:);
E0 = TRI(2,:)-B;
%E0 = E0/sqrt(sum(E0.^2)); %normalize vector
E1 = TRI(3,:)-B;
%E1 = E1/sqrt(sum(E1.^2)); %normalize vector
D = B - P;
a = dot(E0,E0);
b = dot(E0,E1);
c = dot(E1,E1);
d = dot(E0,D);
e = dot(E1,D);
f = dot(D,D);
det = a*c - b*b; % do we have to use abs here?
s = b*e - c*d;
t = b*d - a*e;
% Terible tree of conditionals to determine in which region of the diagram
% shown above the projection of the point into the triangle-plane lies.
if (s+t) <= det
if s < 0
if t < 0
%region4
if (d < 0)
t = 0;
if (-d >= a)
s = 1;
sqrDistance = a + 2*d + f;
else
s = -d/a;
sqrDistance = d*s + f;
end
else
s = 0;
if (e >= 0)
t = 0;
sqrDistance = f;
else
if (-e >= c)
t = 1;
sqrDistance = c + 2*e + f;
else
t = -e/c;
sqrDistance = e*t + f;
end
end
end %of region 4
else
% region 3
s = 0;
if e >= 0
t = 0;
sqrDistance = f;
else
if -e >= c
t = 1;
sqrDistance = c + 2*e +f;
else
t = -e/c;
sqrDistance = e*t + f;
end
end
end %of region 3
else
if t < 0
% region 5
t = 0;
if d >= 0
s = 0;
sqrDistance = f;
else
if -d >= a
s = 1;
sqrDistance = a + 2*d + f;% GF 20101013 fixed typo d*s ->2*d
else
s = -d/a;
sqrDistance = d*s + f;
end
end
else
% region 0
invDet = 1/det;
s = s*invDet;
t = t*invDet;
sqrDistance = s*(a*s + b*t + 2*d) ...
+ t*(b*s + c*t + 2*e) + f;
end
end
else
if s < 0
% region 2
tmp0 = b + d;
tmp1 = c + e;
if tmp1 > tmp0 % minimum on edge s+t=1
numer = tmp1 - tmp0;
denom = a - 2*b + c;
if numer >= denom
s = 1;
t = 0;
sqrDistance = a + 2*d + f; % GF 20101014 fixed typo 2*b -> 2*d
else
s = numer/denom;
t = 1-s;
sqrDistance = s*(a*s + b*t + 2*d) ...
+ t*(b*s + c*t + 2*e) + f;
end
else % minimum on edge s=0
s = 0;
if tmp1 <= 0
t = 1;
sqrDistance = c + 2*e + f;
else
if e >= 0
t = 0;
sqrDistance = f;
else
t = -e/c;
sqrDistance = e*t + f;
end
end
end %of region 2
else
if t < 0
%region6
tmp0 = b + e;
tmp1 = a + d;
if (tmp1 > tmp0)
numer = tmp1 - tmp0;
denom = a-2*b+c;
if (numer >= denom)
t = 1;
s = 0;
sqrDistance = c + 2*e + f;
else
t = numer/denom;
s = 1 - t;
sqrDistance = s*(a*s + b*t + 2*d) ...
+ t*(b*s + c*t + 2*e) + f;
end
else
t = 0;
if (tmp1 <= 0)
s = 1;
sqrDistance = a + 2*d + f;
else
if (d >= 0)
s = 0;
sqrDistance = f;
else
s = -d/a;
sqrDistance = d*s + f;
end
end
end
%end region 6
else
% region 1
numer = c + e - b - d;
if numer <= 0
s = 0;
t = 1;
sqrDistance = c + 2*e + f;
else
denom = a - 2*b + c;
if numer >= denom
s = 1;
t = 0;
sqrDistance = a + 2*d + f;
else
s = numer/denom;
t = 1-s;
sqrDistance = s*(a*s + b*t + 2*d) ...
+ t*(b*s + c*t + 2*e) + f;
end
end %of region 1
end
end
end
% account for numerical round-off error
if (sqrDistance < 0)
sqrDistance = 0;
end
dist = sqrt(sqrDistance);
if nargout>1
PP0 = B + s*E0 + t*E1;
end
