Skip to content

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

@GGooeytoe

Description

@GGooeytoe

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

Metadata

Metadata

Assignees

No one assigned

    Labels

    No labels
    No labels

    Type

    No type

    Projects

    No projects

    Milestone

    No milestone

    Relationships

    None yet

    Development

    No branches or pull requests

    Issue actions