Incorrect result

[goretkin]

using GeometryTypes: HyperRectangle, FlexibleConvexHull, Vec
using CoordinateTransformations: AffineMap
using StaticArrays

geo1 = HyperRectangle{3,Float32}(Float32[-0.08559614, -0.125, 0.0], Float32[0.117596135, 0.25, 0.07049995])

#geo2 = FlexibleConvexHull{Vec{3,Float32}}(Vec{3,Float32}[[-1.4459023, 0.8858041, 0.9851251], [-0.32686943, -0.23770937, -0.0762], [-0.32686943, -0.23770937, 0.204528], [-0.32686943, 0.23770937, -0.0762], [-0.32686943, 0.23770937, 0.204528], [0.32682246, -0.23770937, -0.0762], [0.32682246, -0.23770937, 0.204528], [0.32682246, 0.23770937, -0.0762], [0.32682246, 0.23770937, 0.204528]])

#convex_hull_gg(vertices) = collect(points(vrep(polyhedron(hrep(polyhedron(vrep(vertices)))))))
#geo2_ch = FlexibleConvexHull(map(Vec{3,Float32}, convex_hull_gg(map(Array, geo2.vertices))))

geo2_ch = FlexibleConvexHull{Vec{3,Float32}}(Vec{3,Float32}[[-0.32686943, -0.23770937, 0.204528], [-0.32686943, -0.23770937, -0.0762], [-0.32686943, 0.23770937, -0.0762], [0.32682246, -0.23770937, 0.204528], [0.32682246, -0.23770937, -0.0762], [-1.4459023, 0.8858041, 0.9851251], [0.32682246, 0.23770937, -0.0762], [0.32682246, 0.23770937, 0.204528]])

definitely_inside__Fgeo2 = Vec{3, Float32}(-1.4459023, 0.8858041, 0.9851251)

tx__Fworld__Fgeo1 = AffineMap((@SArray Float32[1.0 -1.3267949e-6 1.7603859e-12; 1.3267949e-6 1.0 1.3267932e-6; -3.5207683e-12 -1.3267932e-6 1.0]), (@SArray Float32[0.28507775, 4.0943226e-7, 1.0193151]))

tx__Fworld__Fgeo2 = AffineMap((@SArray Float32[0.809017 -0.58778524 0.0; 0.58778524 0.809017 0.0; 0.0 0.0 1.0]), (@SArray Float32[2.0, 0.1, 0.0762]))

result = EnhancedGJK.gjk(
  geo1, geo2, tx__Fworld__Fgeo1, tx__Fworld__Fgeo2
)

definitely_inside_Fgeo1 = inv(tx__Fworld__Fgeo1)(tx__Fworld__Fgeo2(definitely_inside__Fgeo2))
@assert GeometryTypes.contains(geo1, definitely_inside_Fgeo1)
@assert result.in_collision # fails


using EnhancedGJK
using MeshCat
using Polyhedra
using StaticArrays: SVector
using GeometryTypes: HyperSphere, Point

function visualize_simplex(vis::Visualizer, simplex)
    p = polyhedron(vrep(simplex))
    setobject!(vis[:simplex], Polyhedra.Mesh(p))
    for (i, point) in enumerate(simplex)
        setobject!(vis["p$i"], HyperSphere(Point(point), convert(eltype(point), 0.03)))
    end
end



convex_mesh(points) = Polyhedra.Mesh(Polyhedra.polyhedron(Polyhedra.vrep(map(Array, points))))

vis = Visualizer()
open(vis)
setobject!(vis[:geo1], geo1)
settransform!(vis[:geo1], tx__Fworld__Fgeo1)
setobject!(vis[:geo2], convex_mesh(geo2.vertices))
settransform!(vis[:geo2], tx__Fworld__Fgeo2)

visualize_simplex(vis[:simplex], result.simplex)

gjk terminates with a degenerate simplex (one point is repeated)