Collision detection not accurate for rays cast

[will-44]

Checklist

• I have searched for similar issues.
• For Python issues, I have tested with the latest development wheel.
• I have checked the release documentation and the latest documentation (for master branch).

Describe the issue

Hello,
I'm trying to use rays to detect collisions with a (complex) mesh. The main goal is to detect the exit from a concavity. The code below is a simple example of the behaviour error that I get. The variable nb_rays can be decreased for a better observation.

1. I cast rays all around an origin with mesh (a cube in the example) near this origin.
2. Those rays are visualized by lines. If a ray hit an obstacle the line associate become red else, it is green.

Steps to reproduce the bug

import open3d as o3d
import numpy as np

# Create the machine mesh
cube_mesh = o3d.geometry.TriangleMesh.create_box(width=1, height=1, depth=1)
cube_mesh = o3d.t.geometry.TriangleMesh.from_legacy(cube_mesh)

# Create the ray casting scene
scene = o3d.t.geometry.RaycastingScene()
cube_id = scene.add_triangles(cube_mesh)

# Create rays: origin and destination
origin = [2, 0.5, 0.5]

# Create a sphere for ray destinations
radius=10
mesh_sphere = o3d.geometry.TriangleMesh.create_sphere(radius=radius, resolution=20)
mesh_sphere.translate(origin)

# Sample destination points on the sphere for the rays
nb_rays = 100000
pcd_ray_dest = mesh_sphere.sample_points_poisson_disk(number_of_points=nb_rays)
dest_np = np.asarray(pcd_ray_dest.points)

# Create lists for rays, lines, and colors
rays = []
line = [[origin[0], origin[1], origin[2]]]
line_indice = []
line_color = []

# Iterate to generate the rays and line (for visual) from origine to the sphere
for index, dest_pt in enumerate(dest_np):
    rays.append([origin[0], origin[1], origin[2], dest_pt[0], dest_pt[1], dest_pt[2]])
    line.append([dest_pt[0], dest_pt[1], dest_pt[2]])
    line_indice.append([0, index])
    line_color.append([1.0, 0.0, 0.0])

rays = o3d.core.Tensor(np.asarray(rays), dtype=o3d.core.Dtype.Float32)
# Perform ray casting
ans = scene.cast_rays(rays)

# Create line set for visualization
lineset = o3d.t.geometry.LineSet()
lineset.point.positions = o3d.core.Tensor(np.asarray(line), dtype=o3d.core.float32)
lineset.line.indices = o3d.core.Tensor(np.asarray(line_indice), dtype=o3d.core.int32)
lineset.line.colors = o3d.core.Tensor(np.asarray(line_color), dtype=o3d.core.float32)

# Get hit results and update line colors
res = ans['t_hit'].numpy()
indice = np.where(res >= radius)[0]
line_color = np.asarray(line_color)
line_color[indice] = [0.0, 1.0, 0.0]
lineset.line.colors = o3d.core.Tensor(line_color, dtype=o3d.core.float32)

# Visualize the scene with machine mesh, coordinate frame, and ray lines
o3d.visualization.draw_geometries([cube_mesh.to_legacy(), lineset.to_legacy()])

Error message

No response

Expected behavior

In the visualization (image 1) we should get a square projected on a sphere, but we can see that the square is not accurate and there is a lot of "noise" in the borders.

More over, image 2 (nb_rays = 100), we can see that some rays in collisions are green (consider not in collision) and some not in collision are red. (consider in collision)

Does my code to visualize got a bug or is there a particular condition with mesh to get correct collisions ?

Open3D, Python and System information

- Operating system: Ubuntu 20.04
- Python version: Python 3.8
- Open3D version: output from python: 0.17.0
- System architecture: x86 
- Is this a remote workstation?: no
- How did you install Open3D?: pip

Additional information

Thank you for your help and for this library (really helpful !)

[benjaminum]

Hi, thank you for the code example.
The format of the rays is [ox,oy,oz,dirx,diry,dirz]. The last 3 entries are a direction vector.

Please try again with these changes:

for index, dest_pt in enumerate(dest_np):
    rays.append([origin[0], origin[1], origin[2], dest_pt[0]-origin[0], dest_pt[1]-origin[1], dest_pt[2]-origin[2]]) # compute direction vector
    line.append([dest_pt[0], dest_pt[1], dest_pt[2]])
    line_indice.append([0, index+1]) # index+1, wrong index was causing random color for some lines
    line_color.append([1.0, 0.0, 0.0])

[will-44]

Thank you !! I work perfectly !