Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Sphere to Mesh nearest point results in nonsense when intersecting #54

Open
GGooeytoe opened this issue Nov 22, 2022 · 0 comments
Open

Comments

@GGooeytoe
Copy link

GGooeytoe commented Nov 22, 2022

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

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

1 participant