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 .