Fix shape conservative advancement normal computation

[ddengster]

Related issue: #501

This PR fixes the frame of reference for computation of points while doing conservative advancement for shapes. There's also an additional test for collisions with splines made from rotated points. If any reviewer/maintainer could advise me on where to place the test please do so!

---

This change is 

[SeanCurtis-TRI]

+@SeanCurtis-TRI for feature review.

Reviewable status: 0 of 2 files reviewed, all discussions resolved (waiting on @SeanCurtis-TRI)

[SeanCurtis-TRI]

Reviewed 1 of 2 files at r1.
Reviewable status: 1 of 2 files reviewed, 4 unresolved discussions (waiting on @ddengster and @SeanCurtis-TRI)

a discussion (no related file):
You've got windows CI failure. In three of the configurations, the test is simply failing (i.e., no collision reported). In the 32-bit release version, we're actually getting an SEH exception which tells me there are some more fundamental issues at play here. I certainly can't find any other reference to SplineMotion in the test directory, which suggests that we have no evidence that it was every particularly happy under windows. :-/ Welcome to the headache of FCL PRs -- lots of supported platforms. :)

Let me know if you need help trying to figure out what's going on.

---

---

include/fcl/narrowphase/detail/traversal/distance/shape_conservative_advancement_traversal_node-inl.h, line 79 at r1 (raw file):

  this->nsolver->shapeDistance(*(this->model1), this->tf1, *(this->model2), this->tf2, &distance, &closest_p1, &closest_p2);

  Vector3<S> n = closest_p2 - closest_p1;

BTW I don't know about you, but I was surprised that this appears the only aspect of the infrastructure that suffered from this problem. I dug a bit and found nothing.

---

test/test_fcl_collision.cpp, line 123 at r1 (raw file):

    r[0], r[1], r[2], r[3]);

  // test collision with unit circles

nit: Could you capitalize "Test", add a period to the end, and swap "circles" for "spheres", please.

---

test/test_fcl_collision.cpp, line 142 at r1 (raw file):

  EXPECT_TRUE(result.is_collide);

nit: extra vertical whitespace. Please delete this empty line.

[SeanCurtis-TRI]

Reviewed 1 of 2 files at r1.
Reviewable status: all files reviewed, 4 unresolved discussions (waiting on @ddengster)

[ddengster]

You've got windows CI failure. In three of the configurations, the test is simply failing (i.e., no collision reported). In the 32-bit release version, we're actually getting an SEH exception which tells me there are some more fundamental issues at play here. I certainly can't find any other reference to SplineMotion in the test directory, which suggests that we have no evidence that it was every particularly happy under windows. :-/ Welcome to the headache of FCL PRs -- lots of supported platforms. :)

I'm building all the libraries/tests on windows x64 Debug and have found out that it's something to do with eigen.
These are the results:

eigen v3.1 - does not compile
eigen v3.2 - gives me the same test failure. I'm guessing this is likely what the CI uses.
eigen v3.3 - works, no test failure.

Perhaps we should up the required eigen versions with this PR?

On a side note, can someone let me know of a hassle-free setup that is able to build fcl + libccd + eigen on windows? Right now I'm literally referencing the CI logs for commands and copying dlls to make things work.

[SeanCurtis-TRI]

Re: windows and FCL - sadly I'm in the same boat. I rarely go near it, and when I do, I go through similar hoops that you are jumping through. Sorry 'bout that.

As for the error, see my note below about alignment.

Reviewed 1 of 1 files at r2.
Reviewable status: all files reviewed, 5 unresolved discussions (waiting on @ddengster)

a discussion (no related file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

You've got windows CI failure. In three of the configurations, the test is simply failing (i.e., no collision reported). In the 32-bit release version, we're actually getting an SEH exception which tells me there are some more fundamental issues at play here. I certainly can't find any other reference to SplineMotion in the test directory, which suggests that we have no evidence that it was every particularly happy under windows. :-/ Welcome to the headache of FCL PRs -- lots of supported platforms. :)

Let me know if you need help trying to figure out what's going on.

In the Debug, windows 32 report, it's providing a classic alignment error:

Assertion failed: (reinterpret_cast<size_t>(array) & 0xf) == 0 && "this assertion is explained here: " "http://eigen.tuxfamily.org/dox-devel/group__TopicUnalignedArrayAssert.html" " **** READ THIS WEB PAGE !!! ****"

Glancing quickly at the test, I didn't see any obvious offenders. I'm inclined to believe it might actually be in the tested class (and given the paucity of unit tests on these classes, that suggests better than even odds). Somewhere, we're storing some structure that requires alignment but isn't getting it.

---

a discussion (no related file):
We're using Reviewble for code review. After addressing a reviewer comment, you should indicate so on the comment.

Any comment with the prefix: "nit", "minor", "fyi", or "btw" can be simply resolved by typing "done" (or just hitting the "Acknowledge" button.

There are actually more semantic conventions, but for now, that's sufficient.

---

[ddengster]

so commit 10e12cc tries to get the numbers synced up and correct, and it's passing the x64 windows builds. I've tracked it down to the normalize function which was not ignoring zero length vectors before normalizing. This issue https://gitlab.com/libeigen/eigen/-/issues/977 points to a fix in 3.3, but the windows CI is using 3.2.9 hence the test failure. Still, this raises the question on whether or not the CI should be updated to eigen 3.3.

Regarding the failing windows 32bit builds I havent gotten around to compiling them yet, so wait for it.

Reviewable status: 1 of 3 files reviewed, 5 unresolved discussions (waiting on @ddengster and @SeanCurtis-TRI)

a discussion (no related file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

We're using Reviewble for code review. After addressing a reviewer comment, you should indicate so on the comment.

Any comment with the prefix: "nit", "minor", "fyi", or "btw" can be simply resolved by typing "done" (or just hitting the "Acknowledge" button.

There are actually more semantic conventions, but for now, that's sufficient.

Done.

---

[traversaro]

On a side note, can someone let me know of a hassle-free setup that is able to build fcl + libccd + eigen on windows?

I typically use vcpkg to install the dependencies of a project I need to use/run tests, and I never had too many problems.

[ddengster]

a discussion (no related file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

In the Debug, windows 32 report, it's providing a classic alignment error:

Assertion failed: (reinterpret_cast<size_t>(array) & 0xf) == 0 && "this assertion is explained here: " "http://eigen.tuxfamily.org/dox-devel/group__TopicUnalignedArrayAssert.html" " **** READ THIS WEB PAGE !!! ****"

Glancing quickly at the test, I didn't see any obvious offenders. I'm inclined to believe it might actually be in the tested class (and given the paucity of unit tests on these classes, that suggests better than even odds). Somewhere, we're storing some structure that requires alignment but isn't getting it.

i've switched from shared_ptr to make_aligned_shared to fix the alignment error. The problem stems from this issue(https://gitlab.com/libeigen/eigen/-/issues/1049) with eigen v3.2

---

[SeanCurtis-TRI]

One math nit, but this looks basically there.

Reviewed 3 of 3 files at r3.
Reviewable status: all files reviewed, 2 unresolved discussions (waiting on @ddengster)

a discussion (no related file):

Previously, ddengster wrote…

Done.

Don't forget to hit "Acknowledge" or type "done". I went ahead and marked myself satisified on the comments you'd addressed but hadn't closed out.

---

---

include/fcl/math/motion/spline_motion-inl.h, line 128 at r3 (raw file):

  Vector3<S> cur_w = Rd[0] * getWeight0(dt) + Rd[1] * getWeight1(dt) + Rd[2] * getWeight2(dt) + Rd[3] * getWeight3(dt);
  S cur_angle = cur_w.norm();
  if (cur_w.squaredNorm() > 0.0)

nit I believe his would be much better as:

S cur_angle = cur_w.norm();
if (cur_angle > 0.0) {
   cur_w /= cur_angle;
}

As it is, you're paying the cost for two redundant dot products and one redundant square root.

[ddengster]

a discussion (no related file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

Don't forget to hit "Acknowledge" or type "done". I went ahead and marked myself satisified on the comments you'd addressed but hadn't closed out.

done

---

[SeanCurtis-TRI]

+@sherm1 Would you like to take a quick look?

Reviewed 1 of 1 files at r4.
Reviewable status: all files reviewed, 1 unresolved discussion (waiting on @ddengster and @sherm1)

[SeanCurtis-TRI]

Reviewable status: complete! all files reviewed, all discussions resolved (waiting on @sherm1)

[sherm1]

Thanks, @ddengster! with one suggestion below.

Reviewed 2 of 3 files at r3, 1 of 1 files at r4.
Reviewable status: all files reviewed, 1 unresolved discussion (waiting on @ddengster)

---

include/fcl/narrowphase/detail/traversal/distance/shape_conservative_advancement_traversal_node-inl.h, line 81 at r4 (raw file):

  Vector3<S> n = closest_p2 - closest_p1;
  if (n.squaredNorm() > 0.0)
    n.normalize();

BTW could avoid the redundant dot product with

const S norm_sq = n.squaredNorm();
if (norm_sq > 0.0)
    n /= sqrt(norm_sq);  // Normalize.

[sherm1]

Reviewed 1 of 1 files at r5.
Reviewable status: all files reviewed, 1 unresolved discussion (waiting on @ddengster)

[sherm1]

Reviewable status: complete! all files reviewed, all discussions resolved (waiting on @ddengster)