python-fcl
python-fcl copied to clipboard
Sphere to Mesh nearest point results in nonsense when intersecting
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."