Performing distance to point queries

[SeanCurtis-TRI]

One of the queries we're using bullet for is distance from point to btCollisionObjects. There is a very real possibility that what we are doing is the wrong way around and would be considered an abuse of Bullet. That said, we've discovered an error. I'll provide a high level discussion of the error here and have code that reproduces the problem attached.

Test Scenario

Create a shape (either sphere, box, or convex hull of the box). It floats above the origin such that the distance from the origin to the nearest point on the shape is 0.25 units. We then take a sequence of test points in the range [0.24, 0.25] (in this case, 101 samples so that the test point starts at a distance of 0.01 and moves closer by 0.0001 at each sample.)

At each query, we look at the known distance, a derived distance (the distance between the query point and the nearest point on the shape to that point), the reported normal from the shape to the query point.

When running the example on the command line, you can specify the test shape as a command line parameter, e.g.,

• ./a.out sphere
• ./a.out box
• ./a.out convexhull

The problem

There are actually several problems:

1) For the box and convex hull, as the point gets closer and closer to the surface (in a monotonically decreasing manner), the reported normal direction flips directions arbitrarily. 2) It should be possible to compute the closest with high precision in these simple scenarios. However, for the convex hull, we are getting an error in distance on the order of 1e-10-1e-12, and for the box, 1e-9-1e-10. This last is particularly surprising. 3) When querying against the sphere shape, the point on the sphere given as closest is always the sphere center, regardless of the radius.

Example Code

bullet_point_query.zip

[SeanCurtis-TRI]

It seems the possible problem with problem three can be found in btConvexShape.cpp. Apparently, the support vertex for a sphere is the origin. One suspects that it should actually be the localDirOrg * radius.

[erwincoumans]

Thanks for the report.

There is a very real possibility that what we are doing is the wrong way around

Forcing very small margins is currently the 'wrong way around' indeed.

in distance on the order of 1e-10-1e-12, and for the box, 1e-9-1e-10. This last is particularly surprising.

There are some hard-coded constants in the GJK/EPA implementation that requires non-zero margins.

Using non-default margins and margin-related settings are likely introducing various issues. The btSphereShape implementation relies on using the entire radius as margin. So this call is not OK for the btSphereShape: sphere->setMargin(1e-9);

In addition, this is also causing issues: convexConvex.setIgnoreMargin(true); please never call this API.

the reported normal direction flips directions arbitrarily.

If this still happens using 'reasonably large' margins, that is considered a bug. It would be good to create some unit tests and collision comparisons for the GJK and EPA to improve its accuracy and reduce/avoid relying on such magic constants.

btQuaternion(1, 0, 0, 0)

Minor thing: the btQuaternion is initialized as [x,y,z,w], so a unit quaterion is [0,0,0,1]

[erwincoumans]

So in a nutshell: don't call setMargin on btSphereShape, is uses the radius as full margin never call setIgnoreMargin the contact normal swap was some issue related to small margins/setIgnoreMargin, this pull request should deal with it: #892 the accuracy of GJK/EPA is indeed around 1e-9/1e-11 in double-precision mode

[SeanCurtis-TRI]

Thanks for the timely feedback. You are always right on top of things. :) I'll make sure our code reflects your suggestions.

So, my general feeling is that doing a distance-to-point query based on convex objects is wasteful. This is definitely an abuse of the infrastructure -- particularly for the primitive types. This specific problem can be solved more simply and with higher accuracy. I believe the code we have that does this was created for reasons of convenience and precision was a lesser concern.

Is there any pre-existing mechanism to support distance to point queries?

[SeanCurtis-TRI]

FTR I took the example code I had attached and made one change: the initialization of the unit quaternion and suddenly the normal oscillation disappeared completely. And that's even leaving the bad call to setIgnoreMargins. So, as "minor things" go, this proved to be quite significant. :)

[erwincoumans]

There is no special case for point versus general convex shape. I don't think using the current infrastructure is wasteful, since the choice was made to use a general purpose algorithm (GJK/EPA) to handle arbitrary convex shapes. Of course if you limit yourself to a few simple cases (sphere, capsule, cylinder, convex polyhedron) then you can optimize much better, and we probably should add some special code to perform point queries for those few special cases.

The GJK/EPA general algorithm can deal with much larger set of arbitrary convex shapes, including margins, Minkowski sums etc. Aside from closest point and penetration depth it also deals with continuous collision detection / ray cast, convex shape cast under constant linear and angular velocity.

[erwincoumans]

Now back to the original 3 points, those are all resolved, right (except point 2, we have to settle for 1e10/1e11 precision). Note that you don't compensate for the margin (which makes the convex shape bigger in some cases), so using 1e-10 for margin will make the 'error' smaller. There are some other tolerances in our GJK/EPA implementation (around 1e-10), so we can expect errors in that range.

1) For the box and convex hull, as the point gets closer and closer to the surface (in a monotonically decreasing manner), the reported normal direction flips directions arbitrarily.

Solved with the pull request.

2) It should be possible to compute the closest with high precision in these simple scenarios. However, for the convex hull, we are getting an error in distance on the order of 1e-10-1e-12, and for the box, 1e-9-1e-10.

1e9/1e11 error is expected, not a bug. If the shapes involved are very different in scale (say performing a closest point query between a cube of 1000km versus a point or very small shape), the absolute error will be larger too, since the GJK/EPA simplices are a combination of support points of both convex shapes, and you'll get catastrophic cancellation in the floating point operations.

3) When querying against the sphere shape, the point on the sphere given as closest is always the sphere center, regardless of the radius.

Don't set the margin for sphere, just use the radius. Should solve it.

[SeanCurtis-TRI]

So, this largely seems to address the issue of numerical stability. The normal isn't flipping (although I'd be loathe to describe this test as exhaustive, but indications look good.) I'd definitely say the original three points have been resolved.

My further comment is about precision. I agree, GJK/EPA should be able to work regardless of the primitives. However, there is a price in that generality -- in both computation cost and precision. So, I concur with you, that it makes sense to have a number of special-case solvers for pairs for which a solution can be found directly (e.g., point and primitives) and then fall back to GJK/EPA for when no special case exists.

[erwincoumans]

Yes, there are already a couple of special cases implemented (sphere-sphere, sphere-box, sphere-triangle etc), but once you go that route, it is best to use the btCollisionWorld API. Then you benefit from broadphase acceleration structures, midphase BVH culling etc.

In btCollisionWorld there are some queries such as rayTest, convexSweepTest, contactTest, contactPairTest, and the contactTest could be optimized to select the best algorithm, while also taking care of the contact culling. If you insist on doing the culling yourself, a contactPairTest could be further optimized to select the most optimal algorithm.

It makes sense to have a deeper discussion how you use the distance to point query, and how such query fits best with Bullet to get best results.

[SeanCurtis-TRI]

I think the rayTest is a good parallel with what we're looking at. The btCollisionWorld has been configured and then we make a query on the state of the world based on arbitrary criteria (the ray). Imagine doing the same thing, but with points.

Right now, our code doesn't exploit the BVH that is internal to bullet. So, when we collide m points against the world with n collision objects, we are doing mn collision computations. That is fundamentally inexcusable. A btCollisionWorld interface would resolve a great deal: it could exploit the broadphase filter algorithm, and seamlessly exploit special case code.

[erwincoumans]

flipping contact normal should be fixed, spheres don't need 'setMargin' and if 1e-10 error residual is too much, please file a new issue with further explanation why/how.