Revert bug introduced in cylinder/half-space intersection.

[amcastro-tri]

We introduced a new bug in #255 when "simplifying" an if statement. A separate unit test should've been introduced in that PR for that change but we didn't and therefore we introduced a bug.

---

This change is 

[amcastro-tri]

@SeanCurtis-TRI for feature review please.
@sherm1 for platform and final merge please.

(Apparently I don't have the bits in this repo to assign you)

[SeanCurtis-TRI]

Oops

---

Reviewed 1 of 1 files at r1.
Review status: all files reviewed at latest revision, 1 unresolved discussion.

---

include/fcl/narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h, line 394 at r1 (raw file):

  {
    Vector3<S> C = dir_z * cosa - new_s2.n;
    if(std::abs(cosa + 1) < halfspaceIntersectTolerance<S>() || std::abs(cosa - 1) < halfspaceIntersectTolerance<S>())

FYI Oops. My bad. That said, the intent is still good and the cheaper test would still be:

if (std::abs(cosa) > 1 - halfSpaceIntersectTolerance<S>()) { ...

The previous incarnation of the test was still quite painful. However, in the name of getting things through, I would accept the revert and if I feel strongly about this, I'll PR it myself later.

---

Comments from Reviewable

[SeanCurtis-TRI]

---

Review status: all files reviewed at latest revision, 1 unresolved discussion.

---

Comments from Reviewable

[sherm1]

I'm happy with the revert until we have a unit test to cover this!

---

Reviewed 1 of 1 files at r1.
Review status: all files reviewed at latest revision, 1 unresolved discussion.

---

Comments from Reviewable

[amcastro-tri]

I believe CI barks at us for the same Mac build error we've seen before. If that's the case, this is ready to merge @sherm1

[SeanCurtis-TRI]

I'm keeping an eye on CI. I'd rather let it finish and then examine the results rather than pushing assuming things are good.

The travis ci failures are due to issues unrelated to this PR

• mac debug: the standard box-capsule failures documented in distance_capsule_box_ccd - test_fcl_capsule_box_1 fails on mac. #260
• mac release: the box-box intersection failures documented in Mac release failures -- box-box intersection #265

Waiting on windows to finish, and then the merge should be good.

---

Review status: all files reviewed at latest revision, 1 unresolved discussion, some commit checks failed.

---

Comments from Reviewable

[amcastro-tri]

Sorry, I thought it had finished

[SeanCurtis-TRI]

You're good. @sherm1 please merge.

---

Review status: all files reviewed at latest revision, 1 unresolved discussion, some commit checks failed.

---

Comments from Reviewable

[jmirabel]

Although this might not be relevant here, for numerical stability reasons, it may be better to use
std::abs(sin(a)) < halfSpaceIntersectTolerance<S>()
instead of
std::abs(cosa) > 1 - halfSpaceIntersectTolerance<S>()
if halfSpaceIntersectTolerance<S>() is very small compared to 1 (especially if you intend to use S==float and not S==double).

[chhtz]

@jmirabel abs(cos(a)) > 1 - eps is equivalent to 2*sin(a/2)^2 < eps || 2*cos(a/2)^2<eps. But I don't think that would help here, since a is not known (and computing a = acos(cosa) really does not help).

[jmirabel]

As cos(a) = P.dot(Q), then abs(sin(a)) = P.cross(Q).norm(). Computing a is indeed not a good idea.

Then, I know that abs(cos(a)) > 1 - eps is not equivalent to abs(sin(a)) < eps. However, this is true at the first order in eps. My concern was that, for small eps, 1-eps might actually be 1. The test abs(cos(a)) > 1 - eps is not correct in this case, while abs(sin(a)) < eps is.

[jmirabel]

Moreover, the if checks if P and Q are aligned, which is preferably checked for with P x Q.

[SeanCurtis-TRI]

I think there are several separate issues in this test:

1. Determining that the axis of the cylinder is parallel with the plane normal. We have an inherent threshold that says "they are parallel enough" (our epsilon).
2. Our ability to distinguish slight changes in relative angle based on the cosine of the angle (near zero or π radians). Obviously, the nature of floating point representation and the cosine function in these regions of the domain mean we lose significant precision near the target values.
3. How "unit length" are our unit-lengths? I.e., given two arbitrary unit-length vectors (represented to our best machine precision), will their dot product truly be the cos(θ) between them?

The suggestion of using sin in place of cos addresses item 2, but doesn't really touch on item 1 or 3.

So, the question is this: does the slop in deciding they are "parallel enough" dwarf the loss of precision due to use of cos? I suspect it does (although, merely a suspicion). Certainly, it doesn't feel that computing a cross product and L2-norm justify the putative benefit of having the ability to distinguish subtle angular changes near parallel.

However, referring to @jmirabel's concern about eps relative to 1, it is critical to make sure that eps is appropriately sized for "unit-length" calculations. It seems that some minor up-scaling of C++'s epsilon would be appropriate for that.

So, in my mind, I'd recommend:

1. Change the test to std::abs(cosa) < 1 - epsilon
2. Confirm epsilon is something on the order of 10 * std::numeric_limits<T>::epsilon() (giving us an order of magnitude of slop).

Thoughts?

[jmirabel]

I agree 2 and 3 are not related. I assumed the vectors were unit-length and something could be done to make sure they are.

I think 1 and 3 are not related in theory but they are related on a computer. Also, to check for how parallel they are, you do not really need the L2-norm but you need the cross product.

Just to be sure I was understood. My main concern was on the numerical precision of 1-eps, which could have a bad effect on the normalization done after. If it is sure that the eps will not be eaten by 1 then, my concern does not hold.

[sherm1]

Random thought (haven't looked closely though): sine and cosine are very flat near 1 -- they actually return 1 for an unpleasantly large range of angles there. Both are extremely well behaved near 0. So rather than checking for cos near 1, which is numerically bad, checking for sin near 0 would be well conditioned. (That's why atan2 is so much better than acos or asin.) Not sure if that makes sense here.

[SeanCurtis-TRI]

Ahhh...I'd missed your concern. Yes, we definitely don't want it to be true that 1 - ε == 1. :)

Now, for the sake of my own sense of clarity, I'd like to touch on something else you said. I can't see how you can avoid the L2-norm. The sin(θ) is not the cross product, but the magnitude of the cross product. You could avoid the square root by taking the squared magnitude (but that's another way to lose precision).

[jmirabel]

@sherm1 Yes, that is why I proposed to use sin instead of cos.

@SeanCurtis-TRI You can use a L1-norm instead of the L2-norm. L1-norm looses the angle interpretation of the test but is faster to compute.

[SeanCurtis-TRI]

@sherm1 You articulated my item 2 wonderfully well -- it's what I mean to suggest, but did it far too indirectly.

@jmirabel I see what you're doing with the L1-norm. And the ratio of L2 and L1 norms is bounded by a constant. But, it becomes an approximation of the sine that is difficult to reason about. It becomes almost impossible to describe what epsilon would mean in that case.

In my mind, it all comes down to what "parallel enough" is. Is it the intent that it means "strictly parallel" while accounting for finite precision? (In that case, the probability of two vectors being exactly parallel to infinite precision is essentially zero.) Or do we mean that, geometrically, there is a cone of deviation that should be considered parallel? If the latter, then we need a basis for defining the cone angle and that angle can inform us if using the cosine is insufficient for our purposes.

EDIT: I just looked it up, epsilon, in this case, is 1e-7.