python-fcl icon indicating copy to clipboard operation
python-fcl copied to clipboard

Sphere to Mesh nearest point results in nonsense when intersecting

Open GGooeytoe opened this issue 2 years ago • 0 comments

Using fcl.distance on a small sphere intersecting the face of the mesh and a BVH representation of that mesh results in nearest points that are not on the mesh OR the sphere. Reproducer below (includes trimesh for visualization; you will need to install pyglet <2.0).

Seems likely this is an FCL, not python-fcl, issue, but thought I should report here first since that's where I encountered it.

import numpy as np
import fcl
import trimesh
#Use Trimesh to visualize the problematic scenario; requires pyglet<2.0
verts = np.array([[1.0, 1.0, 1.0],
                  [2.0, 1.0, 1.0],
                  [1.0, 2.0, 1.0],
                  [1.0, 1.0, 2.0]])
tris  = np.array([[0,2,1],
                  [0,3,2],
                  [0,1,3],
                  [1,2,3]])
query=np.array([1.5,1.25,1.0])
test_rad=0.0001
plot_rad=0.01
mesh=trimesh.Trimesh(vertices=verts,faces=tris)
mesh.visual.face_colors[:,-1]=100
edges=trimesh.load_path(mesh.vertices[mesh.edges_unique])
green_sphere=trimesh.primitives.Sphere(radius=plot_rad,center=query)
green_sphere.visual.face_colors=np.array([0,255,0,255])

bvhmesh=fcl.BVHModel()
bvhmesh.beginModel(len(verts),len(tris))
bvhmesh.addSubModel(verts,tris)
bvhmesh.endModel()
collision_mesh=fcl.CollisionObject(bvhmesh,fcl.Transform())

t=fcl.Transform(np.eye(3),query)
sphere=fcl.Sphere(test_rad)
collision_sphere=fcl.CollisionObject(sphere,t)

request=fcl.DistanceRequest(enable_nearest_points=True,enable_signed_distance=True)
result=fcl.DistanceResult()

ret=fcl.distance(collision_mesh,collision_sphere,request,result)

red_sphere=trimesh.primitives.Sphere(radius=plot_rad,center=result.nearest_points[0])
red_sphere.visual.face_colors=np.array([255,0,0,255])

blue_sphere=trimesh.primitives.Sphere(radius=plot_rad,center=result.nearest_points[1])
blue_sphere.visual.face_colors=np.array([0,0,255,255])

scene=trimesh.Scene([mesh,edges,green_sphere,red_sphere,blue_sphere])
scene.show()

EDIT: Screenshot of result of script. Green is the query point, blue and red are the "nearest points." FCL Sphere to mesh failure

GGooeytoe avatar Nov 22 '22 16:11 GGooeytoe