Capsule AABB collision bug

[YoavFekete]

I found the a few cases where their is a collision between a capsule and aabb but fcl algorithm returns that their is not collision.

Capsule: radius = 36 length = 43.4626 centre = (48.5, 75, 20) quaternion = (-0.203633, -0.0534377, 0.649686, 0.0117178)

AABB side = (72, 26, 40) center = (62, 76, 79)

p.s

alternative Capsule description: cap(Point3d(30.0, 82.0, 29.0), Point3d(67.0, 68.0, 11.0), radius(36.0))

alternative aabb description aabb(Point3d(26.0, 63.0, 59.0), Point3d(98.0, 89.0, 99.0));

[sherm1]

cc @rpoyner-tri (This may affect Drake)

[SeanCurtis-TRI]

Cross reference with #567 as well. It seems to be more of the same.

@YoavFekete A few quick questions to make sure we're all on the same page:

1. Can you report how much penetration you expect between the capsule and the box?
2. Do you mean an AABB (like part of broadphase BVH) or simply a box with an identity rotation?
3. Can you indicate whether your quaternion is w, x, y, z or x, y, z, w.

[YoavFekete]

@SeanCurtis-TRI 1) penetration in this case is big: the point which is the tip of the capsule (30,82,65) and the AABB min max point are min = (26.0, 63.0, 59.0) max= (98.0, 89.0, 99.0) so we get at 10 mm of penetration (if 1 is a mm) sqrt(4^2 + 7^2 + 6^2) = 10.049

2) Box with identity rotation

3)as i created it from a matrix and printed the quaternion using fcl getQuatRotation() so i am not sure so i add here the original rotation matrix as printed by getRotation():

capsule rotation matrix [(0, 0.414149, -0.322116), (0.275278, 0.274219, 0.352567), (-0.851306, 0.322116, 0.414149)]

[YoavFekete]

Well the bug was on my side . it was floating point error that was coused from 2 unitized vector cross product returned that returned a non unitized vector .