Translating a "Point-to-Triangle" distance script from MATLAB to Mathematica

Question

Update - Thanks everyone for your responses! After fixing a problem with vector normalization, the code below now works.

I'm a new user, and I was attempting to port some Mathematica code from MATLAB for calculating the minimum distance between a point in 3-space and a triangle in 3-space. This is partly a result of me wanting to have this functionality in Mathematica, and, I suppose, a good opportunity to practice learning the Mathematica syntax.

The original MATLAB code isn't mine (and can be found here), but appears to work well in MATLAB. Unfortunately, my translated version of the script in Mathematica returns nonsense despite my almost verbatim copy of the script.

I hope this question isn't inappropriate, but can anyone spot any naïve errors during the translation? Note that I had to change D to Dnp to avoid symbol protection issues.

P = {0.5, -0.3, 0.5};

P1 = {0, -1, 0};
P2 = {1, 0, 0};
P3 = {0, 0, 0};

vertices = {P1, P2, P3};

B = P1;
E0 = P2 - B;
E1 = P3 - B;

Dnp = B - P;
a = Dot[E0, E0];
b = Dot[E0, E1];
c = Dot[E1, E1];
d = Dot[E0, Dnp];
e = Dot[E1, Dnp];
f = Dot[Dnp, Dnp];

det = a*c - b*b;
s = b*e - c*d;
t = b*d - a*e;


If[(s + t) <= det,
  If[s < 0,
    If[t < 0,
      If[(d < 0),
        t = 0;
        If[(-d >= a),
         s = 1;
         sqrDistance = a + 2*d + f;
         ,
         s = -d/a;
         sqrDistance = d*s + f;
         ];
        ,
        s = 0;
        If[(e >= 0),
         t = 0;
         sqrDistance = f;
         ,
         If[(-e >= c),
           t = 1;
           sqrDistance = c + 2*e + f;
           ,
           t = -e/c;
           sqrDistance = e*t + f;
           ];
         ];
        ];
      ,
      s = 0;
      If[e >= 0,
       t = 0;
       sqrDistance = f;
       ,
       If[-e >= c,
         t = 1;
         sqrDistance = c + 2*e + f;
         ,
         t = -e/c;
         sqrDistance = e*t + f;
         ];
       ];
      ];
    ,
    If[t < 0,
      t = 0;
      If[d >= 0,
       s = 0;
       sqrDistance = f;
       ,
       If[-d >= a, 
         s = 1;
         sqrDistance = a + 2*d + f;
         ,
         s = -d/a;
         sqrDistance = d*s + f;
         ];
       ];
      ,
      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;
      ];
    ];
  ,
  If[s < 0,
    tmp0 = b + d;
    tmp1 = c + e;
    If[tmp1 > tmp0,
     numer = tmp1 - tmp0;
     denom = a - 2*b + c;
     If[numer >= denom,
      s = 1;
      t = 0;
      sqrDistance = a + 2*d + f;
      ,
      s = numer/denom;
      t = 1 - s;
      sqrDistance = s*(a*s + b*t + 2*d) + t*(b*s + c*t + 2*e) + f;
      ];
     ,
     s = 0;
     If[tmp1 <= 0,
      t = 1;
      sqrDistance = c + 2*e + f;
      ,
      If[e >= 0,
        t = 0;
        sqrDistance = f;
        ,
        t = -e/c;
        sqrDistance = e*t + f;
        ];
      ];
     ];
    ,
    If[t < 0,
      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;
        ,
        t = numer/denom;
        s = 1 - t;
        sqrDistance = s*(a*s + b*t + 2*d) + t*(b*s + c*t + 2*e) + f;
        ];
       ,
       t = 0;
       If[(tmp1 <= 0),
        s = 1;
        sqrDistance = a + 2*d + f;
        ,
        If[(d >= 0),
          s = 0;
          sqrDistance = f;
          ,
          s = -d/a;
          sqrDistance = d*s + f;
          ];
        ];
       ];
      ];
    ,
    numer = c + e - b - d;
    If[numer <= 0,
     s = 0;
     t = 1;
     sqrDistance = c + 2*e + f;
     ,
     denom = a - 2*b + c;
     If[numer >= denom,
      s = 1;
      t = 0;
      sqrDistance = a + 2*d + f;
      ,
      s = numer/denom;
      t = 1 - s;
      sqrDistance = s*(a*s + b*t + 2*d) + t*(b*s + c*t + 2*e) + f;
      ];
     ];
    ];
  ];

If[(sqrDistance < 0), sqrDistance = 0;];

dist = Sqrt[(sqrDistance)]

It's always a good idea to use lower-case letters for your variable names: C, D, E, I, K, N, and O are all reserved. — cormullion, May 16 '13 at 09:54
@cormullion Thanks. :) I was just trying to stay "as true" to the original script as possible to make identification of errors easier. So far there haven't been other flags for reserved variable names. — Richard, May 16 '13 at 09:55
It might interest you to know that there is an (undocumented) built-in function, Graphics`Mesh`PointPolygonDistance[] that you might be able to use... — J. M.'s missing motivation, May 16 '13 at 11:02
@J.M. Doesn't that only work in 2D? The above script should handle the case for a 3D point a triangle embedded in 3-space. — Richard, May 16 '13 at 11:19
you may check this answer http://mathematica.stackexchange.com/questions/20105/efficiently-determining-if-3d-points-are-within-a-surface-composed-of-polygons/20386#20386 — s.s.o, May 16 '13 at 11:21
@cormullion I will work on other approaches, however, I'm really curious why my attempts to adapt the above script, which again works fine in Matlab, seems to fail? — Richard, May 16 '13 at 11:45
I hope this doesn't sound too unfriendly, but this isn't really a code review site; at least, not for long and bulky code. This looks like a rather tedious job and my bet would be a misplaced bracket or another typo error. — Sjoerd C. de Vries, May 16 '13 at 11:56
@SjoerdC.deVries That's completely fair. I think I'll accept cormullion's adaptation of belisarius' (very nice) solution. — Richard, May 16 '13 at 11:58
Thanks! - although it's definitely worth waiting a day or two for the real experts to contribute additional answers (I failed maths at school..). As to your original code, I would say (without knowing anything about MatLab) that it's not a good idea to carry out low-level literal translations, but try to find equivalents for the higher-level tasks (vectors, etc). And that nested If statement would look bad in any language... You could usefully read this question too. — cormullion, May 16 '13 at 12:16
@Richard, you're right, it's a 2D-only solution. At least you have something you can use in 2D. :) — J. M.'s missing motivation, May 16 '13 at 12:31
@cormullion I don't know about you, but I've never had much success translating procedural code: the abstract problem (if not fundamentally procedural in nature) is often so obscured by the nested loops and conditionals that I have difficulty understanding what is actually going on. There is a tendency in papers for the authors to present algorithms procedurally using buggy, ill-defined pseudocode, which one assumes basically paraphrases their actual program. What is really required in these cases is a flowchart, but these seem to be rather passé nowadays. — Oleksandr R., May 17 '13 at 03:37

score 5 · Accepted Answer · edited Apr 13 '17 at 12:55

Sorry I can't make sense of your Matlab code - too many nested Ifs. I don't know whether I've adapted belisarius' answer correctly:

point = RandomInteger[100, {3}];
triangle = RandomInteger[100, {3, 3}] ;
lines = Subsets[triangle, {2}];
nline[{start_, end_}, pt_] := 
  Module[{param = ((pt - start).(end - start))/Norm[end - start]^2}, 
    N@{pt, start + Clip[param, {0, 1}] (end - start)}]

Then:

EuclideanDistance @@ nline[#, point] & /@ lines

{67.0659, 78.7704, 67.0103}

Graphics3D[{
 Sphere[point], 
 Polygon[triangle], 
 Red,
 Line[nline[#, point] & /@ lines]}, 
 Axes -> True]

If I've done it right, upvote his answer!, if not, I'll delete mine...

I'm looking to calculate the minimum distance to the face of the triangle, but still this is very interesting! Thanks! — Richard, May 16 '13 at 11:53

score 2 · Answer 2 · answered May 16 '13 at 12:30

I'm posting this just to show that in Mathematica, it is entirely possible to start from the definitions:

pointTriangleDistance3D[pt_?VectorQ, tri_?MatrixQ] := Module[{p1 = First[tri], s, t}, 
  Sqrt[MinValue[{SquaredEuclideanDistance[pt, p1 + {s, t}.Map[# - p1 &, Rest[tri]]],
                0 <= s <= 1, 0 <= t <= 1, s + t <= 1}, {s, t}]]]

There certainly are more efficient approaches, of course.

Translating a "Point-to-Triangle" distance script from MATLAB to Mathematica

2 Answers2