Distance API

[jslee02]

This change is

[mkoval]

This looks good. I posted a few comments on Reviewable. The only two major ones are:

• The current behavior makes it impossible to compute signed distance because it terminates as soon as the signed distance is <= 0.
• We should verify which Shape pairs work in FCL and either: (1) disable the other pairs, (2) internally convert them to meshes for distance queries, or (3) print an error if we use them in a distance query.

---

Reviewed 23 of 23 files at r1.
Review status: all files reviewed at latest revision, 9 unresolved discussions, some commit checks failed.

---

dart/collision/CMakeLists.txt, line 20 [r1] (raw file):

dart_get_filename_components(header_names "collision headers" ${hdrs})
# TODO: remove below line once the files are completely removed.
list(REMOVE_ITEM header_names "Option.hpp" "Result.hpp")

Why remove the headers from header_names while they still exist?

---

dart/collision/CollisionDetector.cpp, line 64 [r1] (raw file):

  return;
}

It bothers me that these functions have dummy implementations, but collide is left as pure-virtual. I don't see any reason for the inconsistency.

I would prefer that both functions be left as pure virtual and the stub method be implemented implemented by each collision detector that does not support distance queries.

---

dart/collision/CollisionDetector.hpp, line 114 [r1] (raw file):

      CollisionResult* result = nullptr) = 0;

  /// Perform collision check for a single group.

This comment is inaccurate.. It would be good to specify explicitly that (1) this is signed distance and (2) result reflects the minimum distance between any two CollisionObjects in the group.

Please reply if either of my assumptions are incorrect here. It's not entirely obvious what the single-argument distance call should do: the distance from a set to itself is always zero.

---

dart/collision/CollisionGroup.hpp, line 185 [r1] (raw file):

      CollisionResult* result = nullptr);

  /// Perform distance check within this CollisionGroup.

It would be good to clarify the docstrings in this file, too.

---

dart/collision/DistanceOption.hpp, line 45 [r1] (raw file):

struct DistanceOption
{
  bool enableNearestPoints;

Comment?

---

dart/collision/DistanceOption.hpp, line 48 [r1] (raw file):

  /// Distance filter
  std::shared_ptr<DistanceFilter> distanceFilter;

Comment? Also, what does it mean if this is nullptr, like it is by default?

---

dart/collision/fcl/FCLCollisionDetector.cpp, line 1175 [r1] (raw file):

  // Perform narrow-phase check
  fcl::distance(o1, o2, fclRequest, fclResult);

Does this change the fclResult.result pointer or only populate its value? I am concerned that you're storing a pointer by value above, so any change in the pointer will not be reflected below.

---

dart/collision/fcl/FCLCollisionDetector.cpp, line 1189 [r1] (raw file):

  // TODO(JS): Do we don't care what's the actual minimum distance when
  // collision is found? Or should we continue to the narrow check to check
  // every pairs?

We definitely care about the penetration distance. I suggest allowing the user to specify a "minimum distance" that they care about. The user pass -INFINITY if they want to compute signed distance.

---

unittests/testDistance.cpp, line 288 [r1] (raw file):

  auto dart = DARTCollisionDetector::create();
  testSphereSphere(dart);

Doesn't only FCL implement this functionality? How do the Bullet and DART tests pass?

---

Comments from Reviewable

[mkoval]

It appears that this pull request should be merged after #749.

[jslee02]

Review status: 10 of 29 files reviewed at latest revision, 9 unresolved discussions.

---

dart/collision/CMakeLists.txt, line 20 [r1] (raw file):

Previously, mkoval (Michael Koval) wrote…

Why remove the headers from header_names while they still exist?

They are deprecated by `CollisionOption.hpp` and `CollisionResult.hpp`, and `Option.hpp` and `Result.hpp` emit warning messages when included. People wouldn't like to see the warnings when they just included `collision.hpp` or `dart.hpp`.

---

dart/collision/CollisionDetector.hpp, line 114 [r1] (raw file):

Previously, mkoval (Michael Koval) wrote…

This comment is inaccurate.. It would be good to specify explicitly that (1) this is signed distance and (2) result reflects the minimum distance between any two CollisionObjects in the group.

Please reply if either of my assumptions are incorrect here. It's not entirely obvious what the single-argument distance call should do: the distance from a set to itself is always zero.

Done.

---

dart/collision/CollisionGroup.hpp, line 185 [r1] (raw file):

Previously, mkoval (Michael Koval) wrote…

It would be good to clarify the docstrings in this file, too.

Done.

---

dart/collision/DistanceOption.hpp, line 45 [r1] (raw file):

Previously, mkoval (Michael Koval) wrote…

Comment?

Done.

---

dart/collision/DistanceOption.hpp, line 48 [r1] (raw file):

Previously, mkoval (Michael Koval) wrote…

Comment? Also, what does it mean if this is nullptr, like it is by default?

Done.

---

dart/collision/fcl/FCLCollisionDetector.cpp, line 1175 [r1] (raw file):

fcl::distance(o1, o2, fclRequest, fclResult);
Which one do you refer to between fclResult and result? fclResult is referenced and the value is changed by fcl::distance, and result is a pointer but passed in interpreteDistanceResult as a reference.

Sorry if I didn't get you right.

---

dart/collision/fcl/FCLCollisionDetector.cpp, line 1189 [r1] (raw file):

Previously, mkoval (Michael Koval) wrote…

We definitely care about the penetration distance. I suggest allowing the user to specify a "minimum distance" that they care about. The user pass -INFINITY if they want to compute signed distance.

Agree on having "minimum distance" in Option. Also, I think zero would be good for the default value.

---

unittests/testDistance.cpp, line 288 [r1] (raw file):

Previously, mkoval (Michael Koval) wrote…

Doesn't only FCL implement this functionality? How do the Bullet and DART tests pass?

The secret is in the test function:

  if (cd->getType() != collision::FCLCollisionDetector::getStaticType())
  {
    dtwarn << "Aborting test: distance check is not supported by "
           << cd->getType() << ".\n";
    return;
  }

😃

---

dart/collision/CollisionDetector.cpp, line 64 [r1] (raw file):

Previously, mkoval (Michael Koval) wrote…

It bothers me that these functions have dummy implementations, but collide is left as pure-virtual. I don't see any reason for the inconsistency.

I would prefer that both functions be left as pure virtual and the stub method be implemented implemented by each collision detector that does not support distance queries.

Done.

---

Comments from Reviewable

[mkoval]

Reviewed 1 of 5 files at r2, 19 of 19 files at r3.
Review status: all files reviewed at latest revision, 4 unresolved discussions.

---

CHANGELOG.md, line 3 [r3] (raw file):

## DART 6

### DART 6.1.0 (2016-XX-XX)

Missing a CHANGELOG entry for distance measurements. Or do you do this later in the release process?

---

CHANGELOG.md, line 7 [r3] (raw file):

* Misc improvements and bug fixes

  * Added `virtual Shape::getType()` and deprecated `ShapeType Shpae::getShapeType()`: [#724](https://github.com/dartsim/dart/pull/724)

Typo: Shpae. 😄

---

dart/collision/DistanceResult.hpp, line 63 [r3] (raw file):

  /// The nearest point on shapeFrame1. The point is calculated only when
  /// DistanceOption::enableNearestPoints is true, which is false by default.
  Eigen::Vector3d nearestPoint1;

Are the coordinates expressed w.r.t. shapeFrame1 or WorldFrame?

---

dart/collision/fcl/FCLCollisionDetector.cpp, line 1189 [r1] (raw file):

Previously, jslee02 (Jeongseok Lee) wrote…

Agree on having "minimum distance" in Option. Also, I think zero would be good for the default value.

I would prefer `-INFINITY` as the default value since all of the use-cases I can think of (e.g. anything involving optimization) require signed distance.

However, I am not strongly opposed to 0.0 if you have a good justification for that default. Do you have a particular use case in mind?

---

Comments from Reviewable

[jslee02]

Review status: 27 of 29 files reviewed at latest revision, 4 unresolved discussions.

---

CHANGELOG.md, line 3 [r3] (raw file):

Previously, mkoval (Michael Koval) wrote…

Missing a CHANGELOG entry for distance measurements. Or do you do this later in the release process?

I'm switching to updating changelog when the change happens. So yeah this should be updated in this PR.

---

CHANGELOG.md, line 7 [r3] (raw file):

Previously, mkoval (Michael Koval) wrote…

Typo: Shpae. 😄

Done.

---

dart/collision/DistanceResult.hpp, line 63 [r3] (raw file):

Previously, mkoval (Michael Koval) wrote…

Are the coordinates expressed w.r.t. shapeFrame1 or WorldFrame?

It's the world coordinates. Updated the comment.

---

dart/collision/fcl/FCLCollisionDetector.cpp, line 1189 [r1] (raw file):

Previously, mkoval (Michael Koval) wrote…

I would prefer -INFINITY as the default value since all of the use-cases I can think of (e.g. anything involving optimization) require signed distance.

However, I am not strongly opposed to 0.0 if you have a good justification for that default. Do you have a particular use case in mind?

I also don't have a strong objection to `-INFINITY`.

I "anticipated" that people would expect positive value because my first impression was so, but you may have a closer feel to what planning people would expect from this function.

For clarification, we could have both functions as distance with zero threshold and signedDistance with -INFINITY.

---

Comments from Reviewable

[mkoval]

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

---

dart/collision/fcl/FCLCollisionDetector.cpp, line 1189 [r1] (raw file):

Previously, jslee02 (Jeongseok Lee) wrote…

I also don't have a strong objection to -INFINITY.

I "anticipated" that people would expect positive value because my first impression was so, but you may have a closer feel to what planning people would expect from this function.

For clarification, we could have both functions as distance with zero threshold and signedDistance with -INFINITY.

I have generally seen signed distance used in planning and perception code. The problem with the "intuitive" definition of distance is that: (1) no gradient information is available when you're in contact and (2) there is a discontinuity in the derivative at zero.

I see some implementation issuers with defining separate signedDistance and distance functions:

• distance can still return negative values in the current implementation, so the behavior is unintuitive (I would expect 0.0 for any objects in collision).
• distance would have to ignore the minimumDistanceThreshold option
• signedDistance would not compute signed distance with the default value of minimumDistanceThreshold

Overall, I would prefer keeping one function and change the default value of minimumDistanceThreshold to -INFINITY. I slightly favor the name distance over signedDistance because it only computes signed distance when minimumDistanceThreshold is -INFINITY. I do not feel too strongly about this aspect, though.

Maybe @cdellin, @psigen, or @mklingen have a stronger opinion?

---

Comments from Reviewable

[cdellin]

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

---

dart/collision/fcl/FCLCollisionDetector.cpp, line 1189 [r1] (raw file):

Previously, mkoval (Michael Koval) wrote…

I have generally seen signed distance used in planning and perception code. The problem with the "intuitive" definition of distance is that: (1) no gradient information is available when you're in contact and (2) there is a discontinuity in the derivative at zero.

I see some implementation issuers with defining separate signedDistance and distance functions:

• distance can still return negative values in the current implementation, so the behavior is unintuitive (I would expect 0.0 for any objects in collision).
• distance would have to ignore the minimumDistanceThreshold option
• signedDistance would not compute signed distance with the default value of minimumDistanceThreshold

Overall, I would prefer keeping one function and change the default value of minimumDistanceThreshold to -INFINITY. I slightly favor the name distance over signedDistance because it only computes signed distance when minimumDistanceThreshold is -INFINITY. I do not feel too strongly about this aspect, though.

Maybe @cdellin, @psigen, or @mklingen have a stronger opinion?

I just chatted with @mkoval, and what I think we arrived at was that the function should be called `distance`, and that it should always return a clamped distance, with the `minimumDistanceThreshold` defaulting `0.0`. (That is, if the actual distance is less than `minimumDistanceThreshold`, the function will return `minimumDistanceThreshold`.) This is easy to motivate and understand in the documentation. Separately, the caller may want to know the first distance found that caused the clamp to occur, so there should be some mechanism for the caller to discover this information (similar to how the caller can ask which pair of bodies caused a collision check to shortcut to true).

---

Comments from Reviewable

[mkoval]

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

---

dart/collision/fcl/FCLCollisionDetector.cpp, line 1189 [r1] (raw file):

Previously, cdellin wrote…

I just chatted with @mkoval, and what I think we arrived at was that the function should be called distance, and that it should always return a clamped distance, with the minimumDistanceThreshold defaulting 0.0. (That is, if the actual distance is less than minimumDistanceThreshold, the function will return minimumDistanceThreshold.) This is easy to motivate and understand in the documentation. Separately, the caller may want to know the first distance found that caused the clamp to occur, so there should be some mechanism for the caller to discover this information (similar to how the caller can ask which pair of bodies caused a collision check to shortcut to true).

I agree with @cdellin. To summarize:

• Have one function called distance.
• Clamp the output of the function to std::max(actualDistance, minimumDistanceThreshold).
• Expose the true actualDistance value (even if it's less than minimumDistanceThreshold) as a field in DistanceResult.
• Set the default minimumDistanceThreshold to 0.0.
• Update the documentation accordingly.

@jslee02 What do you think?

---

Comments from Reviewable

[cdellin]

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

---

dart/collision/fcl/FCLCollisionDetector.cpp, line 1189 [r1] (raw file):

Previously, mkoval (Michael Koval) wrote…

I agree with @cdellin. To summarize:

• Have one function called distance.
• Clamp the output of the function to std::max(actualDistance, minimumDistanceThreshold).
• Expose the true actualDistance value (even if it's less than minimumDistanceThreshold) as a field in DistanceResult.
• Set the default minimumDistanceThreshold to 0.0.
• Update the documentation accordingly.

@jslee02 What do you think?

And by `actualDistance`, we mean the first distance found that is lower than `minimumDistanceThreshold`, correct? Not the actual distance. (This would be as expensive to find as with no clamping.)

---

Comments from Reviewable

[mkoval]

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

---

dart/collision/fcl/FCLCollisionDetector.cpp, line 1189 [r1] (raw file):

Previously, cdellin wrote…

And by actualDistance, we mean the first distance found that is lower than minimumDistanceThreshold, correct? Not the actual distance. (This would be as expensive to find as with no clamping.)

Yes, that's what I mean. The fact that the best name for it is `firstDistanceLessThanMinimumDistanceThreshold` is a good indication that it's a _bad value to return by default_. 😓

---

Comments from Reviewable

[jslee02]

Review status: 24 of 25 files reviewed at latest revision, 1 unresolved discussion.

---

dart/collision/fcl/FCLCollisionDetector.cpp, line 1189 [r1] (raw file):

Previously, mkoval (Michael Koval) wrote…

Yes, that's what I mean. The fact that the best name for it is firstDistanceLessThanMinimumDistanceThreshold is a good indication that it's a bad value to return by default. 😓

I ​agree with you but have some questions:

What's the motivation behind of clamping? Would it be worthwhile even if the distance doesn't refer to actual distance? Also, I would concern the corner case of that there is no distance less than minimumDistanceThreshold. In this case, distance would return minimumDistanceThreshold when every two points (from distinct objects) in the group are farther than minimumDistanceThreshold.

Do you mean that distance returns the clamped dista​nce and also have it in DistanceResult?

Why do we prefer 0.0 for minimumDistanceThreshold now?

---

Comments from Reviewable

[cdellin]

Review status: 24 of 25 files reviewed at latest revision, 1 unresolved discussion.

---

dart/collision/fcl/FCLCollisionDetector.cpp, line 1189 [r1] (raw file):

Previously, jslee02 (Jeongseok Lee) wrote…

I ​agree with you but have some questions:

What's the motivation behind of clamping? Would it be worthwhile even if the distance doesn't refer to actual distance? Also, I would concern the corner case of that there is no distance less than minimumDistanceThreshold. In this case, distance would return minimumDistanceThreshold when every two points (from distinct objects) in the group are farther than minimumDistanceThreshold.

Do you mean that distance returns the clamped dista​nce and also have it in DistanceResult?

Why do we prefer 0.0 for minimumDistanceThreshold now?

My understanding is that there are situations where the caller doesn't care what the distance is if it's below a certain value (whether that threshold is negative or positive, e.g. padding). For example, Mike gave the sample of covering a path with free C-space bubbles, where the distance is only useful if it is positive. This can be represented elegantly by modifying the API to return a clamped value. In these cases, the distance function can often return more quickly.

I must admit I don't understand your corner case example. If there is no distance less than minimumDistanceThreshold, then the actual distance d, which would be returned with no clamping (i.e. if minimumDistanceThreshold = -INFINITY), would be greater than minimumDistanceThreshold, so the function would return d, and no clamping would be performed.

You might have been referring to the opposite bound -- a maximumDistanceThreshold. There, if d were larger than this value (e.g. if every two points from distinct objects are farther than maximumDistanceThreshold, then the result would clamp to this value. (In fact, if you squint, you could argue that a binary collision checker is equivalent to a clamped distance call, with minimumDistanceThreshold = -EPSILON and maximumDistanceThreshold = +EPSILON.)

I'll let @mkoval give his motivation for the extra data that would be available in DistanceResult -- I think the idea is that this would be the first discovered unclamped value outside of the clamping range, but not necessarily the minimum distance across all bodies, because the function returned early.

We also felt that a default of 0.0 might make more sense for a function called distance (as opposed to signedDistance), although I'm personally flexible on that.

---

Comments from Reviewable

[jslee02]

Review status: 24 of 25 files reviewed at latest revision, 1 unresolved discussion.

---

dart/collision/fcl/FCLCollisionDetector.cpp, line 1189 [r1] (raw file):

Previously, cdellin wrote…

My understanding is that there are situations where the caller doesn't care what the distance is if it's below a certain value (whether that threshold is negative or positive, e.g. padding). For example, Mike gave the sample of covering a path with free C-space bubbles, where the distance is only useful if it is positive. This can be represented elegantly by modifying the API to return a clamped value. In these cases, the distance function can often return more quickly.

I must admit I don't understand your corner case example. If there is no distance less than minimumDistanceThreshold, then the actual distance d, which would be returned with no clamping (i.e. if minimumDistanceThreshold = -INFINITY), would be greater than minimumDistanceThreshold, so the function would return d, and no clamping would be performed.

You might have been referring to the opposite bound -- a maximumDistanceThreshold. There, if d were larger than this value (e.g. if every two points from distinct objects are farther than maximumDistanceThreshold, then the result would clamp to this value. (In fact, if you squint, you could argue that a binary collision checker is equivalent to a clamped distance call, with minimumDistanceThreshold = -EPSILON and maximumDistanceThreshold = +EPSILON.)

I'll let @mkoval give his motivation for the extra data that would be available in DistanceResult -- I think the idea is that this would be the first discovered unclamped value outside of the clamping range, but not necessarily the minimum distance across all bodies, because the function returned early.

We also felt that a default of 0.0 might make more sense for a function called distance (as opposed to signedDistance), although I'm personally flexible on that.

Thank you for the answers. Now I get a ​better understanding.

The corner case example was incorrect. Somehow I thought every distance less than the threshold is rejected.

Before I make changes based on the discussion, I would like to also replace the name minimumDistanceThreshold with minimumDistanceResolution. The minimumDistanceThreshold sounds like a minimum distance for a valid distance for the check (like I was confused).

---

Comments from Reviewable

[psigen]

dart/collision/fcl/FCLCollisionDetector.cpp, line 1189 [r1] (raw file):

Previously, jslee02 (Jeongseok Lee) wrote…

Thank you for the answers. Now I get a ​better understanding.

The corner case example was incorrect. Somehow I thought every distance less than the threshold is rejected.

Before I make changes based on the discussion, I would like to also replace the name minimumDistanceThreshold with minimumDistanceResolution. The minimumDistanceThreshold sounds like a minimum distance for a valid distance for the check (like I was confused).

I'm not sure `minimumDistanceResolution` is a good name.

Resolution often implies a discretization level in the collision check. For example, OpenRAVE uses the terminology DOFResolution to describe the discretization of joint DOF values that are queried when doing collision checking. It also has similar connotations in voxelized fields, including signed distance fields based on structures like occupancy grids.

---

Comments from Reviewable

[jslee02]

Review status: 24 of 25 files reviewed at latest revision, 1 unresolved discussion.

---

dart/collision/fcl/FCLCollisionDetector.cpp, line 1189 [r1] (raw file):

Previously, psigen (Pras Velagapudi) wrote…

I'm not sure minimumDistanceResolution is a good name.

Resolution often implies a discretization level in the collision check. For example, OpenRAVE uses the terminology DOFResolution to describe the discretization of joint DOF values that are queried when doing collision checking. It also has similar connotations in voxelized fields, including signed distance fields based on structures like occupancy grids.

That makes sense. `minimumDistanceResolution` got two down votes today (and now another). 😓

---

Comments from Reviewable

[jslee02]

Review status: 24 of 25 files reviewed at latest revision, 1 unresolved discussion.

---

dart/collision/fcl/FCLCollisionDetector.cpp, line 1189 [r1] (raw file):

Previously, jslee02 (Jeongseok Lee) wrote…

That makes sense. minimumDistanceResolution got two down votes today (and now another). 😓

Those are good examples the term of "resolution" though. Thanks!

---

Comments from Reviewable

[psigen]

dart/collision/fcl/FCLCollisionDetector.cpp, line 1189 [r1] (raw file):
Ok, I see.

double distance(scene, threshold): Return the minimum distance between object pairs in the scene, clamped to be at least threshold.

This seems good.

---

Comments from Reviewable

[psigen]

dart/collision/fcl/FCLCollisionDetector.cpp, line 1192 [r1] (raw file):

Previously, cdellin wrote…

(By the way, I really don't like calling this actualDistance -- it's really actualDistanceFoundSoFarBeforeShortcutting.)

I think you were getting confused here. I was talking about setting `distanceTolerance = -0.5` and using values of distance _greater_ than this value. Which is unaffected by clamping. But is also unaffected by _not_ clamping.

---

Comments from Reviewable

[psigen]

dart/collision/fcl/FCLCollisionDetector.cpp, line 1189 [r1] (raw file):

Previously, psigen (Pras Velagapudi) wrote…

Ok, I see.

double distance(scene, threshold): Return the minimum distance between object pairs in the scene, clamped to be at least threshold.

This seems good.

I like @cdellin's API. Just calling it `distanceThreshold`, `minDistance` or even `threshold` seems good to me. While people might be unsure what it means at first, we can easily explain it in the docs and comments. It is also the only behavior for this parameter that really seems to make sense in this context, given our extensive discussion here.

And more importantly, by using fewer words, we won't imply that this parameter does something that it doesn't!

---

Comments from Reviewable

[psigen]

dart/collision/fcl/FCLCollisionDetector.cpp, line 1192 [r1] (raw file):

Previously, psigen (Pras Velagapudi) wrote…

I think you were getting confused here. I was talking about setting distanceTolerance = -0.5 and using values of distance greater than this value. Which is unaffected by clamping. But is also unaffected by not clamping.

@cdellin that's a good point, it's usually just some intermediate value in most detectors.

---

Comments from Reviewable

[cdellin]

Review status: 24 of 25 files reviewed at latest revision, 2 unresolved discussions.

---

dart/collision/fcl/FCLCollisionDetector.cpp, line 1192 [r1] (raw file):

I think you were getting confused here. I was talking about setting distanceTolerance = -0.5 and using values of distance greater than this value.

Ah, gotcha. My mistake!

---

Comments from Reviewable

[cdellin]

Review status: 24 of 25 files reviewed at latest revision, 2 unresolved discussions.

---

dart/collision/fcl/FCLCollisionDetector.cpp, line 1189 [r1] (raw file):

Previously, psigen (Pras Velagapudi) wrote…

I like @cdellin's API. Just calling it distanceThreshold, minDistance or even threshold seems good to me. While people might be unsure what it means at first, we can easily explain it in the docs and comments. It is also the only behavior for this parameter that really seems to make sense in this context, given our extensive discussion here.

And more importantly, by using fewer words, we won't imply that this parameter does something that it doesn't!

Sounds good! One other idea might to call it `minDistance` or similar (lower bound, etc), because it allows us to introduce a similar `maxDistance` clamping threshold in the future. I can imagine cases where callers only care about distances if they're below a threshold, and the same optimizations can be employed by the implementation.

---

Comments from Reviewable

[psigen]

dart/collision/fcl/FCLCollisionDetector.cpp, line 1189 [r1] (raw file):

Previously, cdellin wrote…

Sounds good! One other idea might to call it minDistance or similar (lower bound, etc), because it allows us to introduce a similar maxDistance clamping threshold in the future. I can imagine cases where callers only care about distances if they're below a threshold, and the same optimizations can be employed by the implementation.

**Unrelated to current API discussion:** @cdellin: One curious thing to note is that clamped `minDistance`/`maxDistance` parameters would not provide any reasonable way to converge to binary collision checking: setting both to the same value wouldn't actually produce a distinguishable result, _because of the clamping_. It would simply always return `distance` = `minDistance` = `maxDistance`.

---

Comments from Reviewable

[cdellin]

Review status: 24 of 25 files reviewed at latest revision, 2 unresolved discussions.

---

dart/collision/fcl/FCLCollisionDetector.cpp, line 1189 [r1] (raw file):

Previously, psigen (Pras Velagapudi) wrote…

Unrelated to current API discussion:
@cdellin: One curious thing to note is that clamped minDistance/maxDistance parameters would not provide any reasonable way to converge to binary collision checking: setting both to the same value wouldn't actually produce a distinguishable result, because of the clamping. It would simply always return distance = minDistance = maxDistance.

@pkv Well, that's certainly a good point. (Of course you could hack it by passing `minDistance = -std::numeric_limits::min()` and `maxDistance = std::numeric_limits::min()`.) Alternatively, I could also imagine providing in the `Result` structure flags for whether clamping occurred.

---

Comments from Reviewable

[mkoval]

Review status: 24 of 25 files reviewed at latest revision, 2 unresolved discussions.

---

dart/collision/fcl/FCLCollisionDetector.cpp, line 1189 [r1] (raw file):

Previously, cdellin wrote…

@pkv Well, that's certainly a good point. (Of course you could hack it by passing minDistance = -std::numeric_limits<double>::min() and maxDistance = std::numeric_limits<double>::min().) Alternatively, I could also imagine providing in the Result structure flags for whether clamping occurred.

Whew. It sounds like we've come to a consensus. 😅

This is my understanding:

• CollisionDetector has one member function called distance
• distance returns a double that is minimum distance between any pair of CollisionObjects being checked clamped to minDistance
• minDistance defaults to 0.0
• DistanceResult has a getDistance member function that returns the clamped distance and a get getUnclampedDistance member function that contains the unclamped distance (along with a warning that the value is implementation defined).

@psigen @cdellin @jslee02 Any objections?

---

Comments from Reviewable

[cdellin]

Review status: 24 of 25 files reviewed at latest revision, 2 unresolved discussions.

---

dart/collision/fcl/FCLCollisionDetector.cpp, line 1189 [r1] (raw file):

Previously, mkoval (Michael Koval) wrote…

Whew. It sounds like we've come to a consensus. 😅

This is my understanding:

• CollisionDetector has one member function called distance
• distance returns a double that is minimum distance between any pair of CollisionObjects being checked clamped to minDistance
• minDistance defaults to 0.0
• DistanceResult has a getDistance member function that returns the clamped distance and a get getUnclampedDistance member function that contains the unclamped distance (along with a warning that the value is implementation defined).

@psigen @cdellin @jslee02 Any objections?

I'm `+1`, but could we have `DistanceResult::getUnclampedDistance` return not only the violating distance (or bound on the distance, whatever the implementation calculated), but also the names or IDs of the bodies that caused the shortcut? Similar to the way you can ask the collision detector which pair of bodies caused a collision.

---

Comments from Reviewable

[mkoval]

Reviewed 1 of 1 files at r6.
Review status: all files reviewed at latest revision, 2 unresolved discussions.

---

dart/collision/fcl/FCLCollisionDetector.cpp, line 1189 [r1] (raw file):

Previously, cdellin wrote…

I'm +1, but could we have DistanceResult::getUnclampedDistance return not only the violating distance (or bound on the distance, whatever the implementation calculated), but also the names or IDs of the bodies that caused the shortcut? Similar to the way you can ask the collision detector which pair of bodies caused a collision.

That's already available in `DistanceResult`:

https://github.com/dartsim/dart/blob/js/distance/dart/collision/DistanceResult.hpp#L45

---

Comments from Reviewable

[cdellin]

Review status: all files reviewed at latest revision, 2 unresolved discussions.

---

dart/collision/fcl/FCLCollisionDetector.cpp, line 1189 [r1] (raw file):

Previously, mkoval (Michael Koval) wrote…

That's already available in DistanceResult:

https://github.com/dartsim/dart/blob/js/distance/dart/collision/DistanceResult.hpp#L45

Hmm, I generally think this is fine, but actually calling it `getUnclampedDistance` implies to me that if I were to set the `minDistance` parameter to `-INFINITY`, that would be the distance I would get. What about something like `getClampViolatingDistance` or something? (I'm sure there are better ideas out there!)

---

Comments from Reviewable

[psigen]

dart/collision/fcl/FCLCollisionDetector.cpp, line 1189 [r1] (raw file):

Previously, cdellin wrote…

Hmm, I generally think this is fine, but actually calling it getUnclampedDistance implies to me that if I were to set the minDistance parameter to -INFINITY, that would be the distance I would get. What about something like getClampViolatingDistance or something? (I'm sure there are better ideas out there!)

If the value is implementation defined, would it make sense for this not to be in `DistanceResult` at all, but instead be in another structure that is dependent on the CollisionDetector (and possible requires a `dynamic_cast<>` at some point)? This would be similar to how `Joint` information works. If the case that @mkoval and @cdellin made before actually true, then this unclamped distance is already an implementation specific value which is meaningless without knowing which detector provided it, and may depend on other _implementation specific values that may need to be made available_.

In short, I think if we add this variable into the interface, we open the door to needing other random fields because we really don't know what this value will mean. It might require that we return additional geometric information, or boolean flags about how the check ended up being done, etc.

---

Comments from Reviewable

[mkoval]

Review status: all files reviewed at latest revision, 2 unresolved discussions.

---

dart/collision/fcl/FCLCollisionDetector.cpp, line 1189 [r1] (raw file):

Previously, psigen (Pras Velagapudi) wrote…

If the value is implementation defined, would it make sense for this not to be in DistanceResult at all, but instead be in another structure that is dependent on the CollisionDetector (and possible requires a dynamic_cast<> at some point)? This would be similar to how Joint information works. If the case that @mkoval and @cdellin made before actually true, then this unclamped distance is already an implementation specific value which is meaningless without knowing which detector provided it, and may depend on other implementation specific values that may need to be made available.

In short, I think if we add this variable into the interface, we open the door to needing other random fields because we really don't know what this value will mean. It might require that we return additional geometric information, or boolean flags about how the check ended up being done, etc.

I am fine with removing the unclamped field entirely. We can always add it in a future version of DART if there proves to be a need for it.

---

Comments from Reviewable

[jslee02]

Sorry for the delayed update.

I basically agree with the consensus but have one concern on it. DistanceResult provides three kinds (but they are all related) information: distance, objects, nearest points. If we removing the unclamped field completely then it could cause some confusion by having inconsistent distance and the nearest points.

From my perspective, the basic goal of distance query is to find minimum distance across all the objects in the collision group, and optionally the nearest points that forms a line segment with the distance. The lower bound option is for perfomance boosting in case the caller doesn't care about distance below a certain value. In that sense, I would prefer having at least one set of API that returns consistent distance, object pair, and nearest points. This could be done by having following fields: minDistance, shapeFrame1/2, nearestPoint1/2. minDistance (called unclamped distance or actual distance) is the minimum distance among being checked objects. We could have getMinDistance for this field, and getDistance returns the clamped distance.

Maybe I have a different view on the distance query so probably I'm not completely convinced to that clamped distance is the default distance. Also, I might have something missed from the above long discussion. Please let me know if there is.

[cdellin]

@jslee02 Good point -- those additional return values (colliding objects and nearest points) are only valid (and really should only be populated) when the return value is not clamped. (That is, in the case that dist == minDistance and therefore there was a clamp performed, the computation probably shortcutted early, so any objects or nearest points are probably not correct anyway.) So perhaps the DistanceResult could include a flag for whether clamping occurred, and only populate those values if clamped == false? If the user sets minDistance = -INFINITY, then no clamping will ever be performed, and those values can always be trusted.

[mkoval]

I like @cdellin's suggestion of adding a flag to the Result struct that indicates whether or not clamping occurred. This correctly captures the semantic that the fields you mentioned (nearest points, etc) points are valid as long as the actual distance is greater than minDistance.

Like @cdellin said, this condition is always satisfied when minDistance = -infinity

[jslee02]

I updated the distance API according to the discussion in the last commit.

I also like to have an indicator for whether the distance was clamped. For that Distance::isMinDistanceClamped() is added that returns true if minDistance and unclampedMinDistance are equal rather than using a flag. This is because comparing the two distance is a bit simpler to implement when DistanceResult could be nullptr. Comparing floating values would be more cost than checking boolean, but don't think this is performance critical.

Thanks for the comments @mkoval , @cdellin , and @psigen . Please take a look at the changes and let me know if it looks good to merge.

[cdellin]

Sounds (and looks) good to me, thanks @jslee02!

[mkoval]

LGTM. 👍