Fix extractClosestPoint method, reset dir variable

[Levi-Armstrong]

It was difficult to track down but occasionally I would get invalid nearest points being meters off. It turns out dir variable gets normalized here. Which becomes an issue at two places. The first, if it makes it into this if statement here it passes an incorrect point to be extracted. The second place, is on the next interation if enters this function and there is not improvement the dir is not recalculated shown here. Which then if it enters this if statment here it would pass an incorrect point to be extracted.

---

This change is 

[Levi-Armstrong]

@avalenzu I have noticed that the EPA technique for extracting nearest points is not always accurate. It looks like it just loops over the vertices's and finds the closest to the witness. Could your method for extracting nearest be applied here to get a more accurate nearest point when in collision?

[avalenzu]

Potentially? extractClosestPoints() just takes a point contained in the v portion of a ccd simplex, and computes the corresponding points in the v1 and v2 portion of the simplex. If that describes what you need in the EPA case, then it should work. Just to make sure I've understand your question though, these are the points of deepest penetration, right?

[Levi-Armstrong]

If that describes what you need in the EPA case, then it should work. Just to make sure I've understand your question though, these are the points of deepest penetration, right?

That is correct. I am testing it now and will let you know how it works.

[Levi-Armstrong]

Ping?

[SeanCurtis-TRI]

I'll review this tomorrow morning as well....

[SeanCurtis-TRI]

(oops -- sorry, accidentally clicked close by a gnat's wing.)

@Levi-Armstrong you'll need to rebase this as well.

[Levi-Armstrong]

I just rebased.

[SeanCurtis-TRI]

A few things:

1. The conversation above deals with a lot of details of the collide() method (e.g., EPA). This PR seems to deal with the distance() method. So, I'm worried about misunderstanding what I'm seeing here or reading above. I could use some elaboration (particularly since @avalenzu asked if this was regarding "deepest penetration", and I believe that this functionality is about non-penetrating.) Specifically, this function is called here; the branch is labeled "not in collision". So, I want to make sure we're all having the same conversation.

2. This desperately needs a unit test that fails without this correction, but passes with it. While the change is simple, the implication is much more arduous without such a test.

So, I'm going to lob this back and ask for a unit test (perhaps included in test_fcl_distance.cpp?)

I will note, however, that this change causes the mac CI failure which has been plaguing us to go away. So, that's a strong indicator. But we do need a unit test that would fail without this fix on any OS.

---

Reviewed 1 of 1 files at r1.
Review status: all files reviewed at latest revision, all discussions resolved, some commit checks failed.

---

Comments from Reviewable

[hongkai-dai]

I believe this issue is resolved in #305, #296 and #290.

[SeanCurtis-TRI]

Agreed. @Levi-Armstrong I believe your problem is now resolved in master. Can you confirm that and then we can close this PR.

Reviewable status: complete! all files reviewed, all discussions resolved

[Levi-Armstrong]

Sure, I will check and confirm that my issue is resolved.

[SeanCurtis-TRI]

Thanks. I appreciate it.

Reviewable status: complete! all files reviewed, all discussions resolved

[Levi-Armstrong]

I was able to test it but to completely resolve the issue I had to include #288 also?