Continuous Collision With Simple Mesh Example Fails

[weshoke]

import numpy as np
import fcl
from trimesh import Trimesh


def print_collision_result(o1_name, o2_name, result):
    print('Collision between {} and {}:'.format(o1_name, o2_name))
    print('-'*30)
    print('Collision?: {}'.format(result.is_collision))
    print('Number of contacts: {}'.format(len(result.contacts)))
    print('')


def print_continuous_collision_result(o1_name, o2_name, result):
    print('Continuous collision between {} and {}:'.format(o1_name, o2_name))
    print('-'*30)
    print('Collision?: {}'.format(result.is_collide))
    print('Time of collision: {}'.format(result.time_of_contact))
    print('')


def print_distance_result(o1_name, o2_name, result):
    print('Distance between {} and {}:'.format(o1_name, o2_name))
    print('-'*30)
    print('Distance: {}'.format(result.min_distance))
    print('Closest Points:')
    print(result.nearest_points[0])
    print(result.nearest_points[1])
    print('')


def create_fcl_bvh(mesh: Trimesh) -> fcl.BVHModel:
    bvh = fcl.BVHModel()
    err = bvh.beginModel(len(mesh.vertices), len(mesh.faces))
    err = bvh.addSubModel(mesh.vertices, mesh.faces)
    err = bvh.endModel()
    return bvh


def create_fcl_transform(homog) -> fcl.Transform:
    res = fcl.Transform(homog[:3, :3], homog[:3, 3])
    return res


def print_continuous_collision_result(o1_name, o2_name, result):
    print('Continuous collision between {} and {}:'.format(o1_name, o2_name))
    print('-' * 30)
    print('Collision?: {}'.format(result.is_collide))
    print('Time of collision: {}'.format(result.time_of_contact))
    print('')


def print_collision_result(o1_name, o2_name, result):
    print('Collision between {} and {}:'.format(o1_name, o2_name))
    print('-'*30)
    print('Collision?: {}'.format(result.is_collision))
    print('Number of contacts: {}'.format(len(result.contacts)))
    print('')


def check_collision(obj1, obj2):
    request = fcl.CollisionRequest()
    result = fcl.CollisionResult()
    ret = fcl.collide(obj1, obj2, request, result)
    print_collision_result("obj1", "obj2", result)


vertices = np.array([
    [-1.0, -1.0, 0.0],
    [1.0, -1.0, 0.0],
    [-1.0, 1.0, 0.0],
    [1.0, 1.0, 0.0],
    [-1.0, -1.0, 1.0],
    [1.0, -1.0, 1.0],
    [-1.0, 1.0, 1.0],
    [1.0, 1.0, 1.0]
])
faces = np.array([
    [3, 1, 0],
    [3, 0, 2],
    [0, 1, 5],
    [0, 5, 4],
    [1, 3, 7],
    [1, 7, 5],
    [3, 2, 6],
    [3, 6, 7],
    [2, 0, 4],
    [2, 4, 6],
    [4, 5, 7],
    [4, 7, 6],
])


mesh1 = Trimesh(vertices=vertices, faces=faces)
mesh2 = mesh1.copy()

mesh1.apply_translation((0., 0., 3.))
mesh2.apply_scale((2., 2., 0.5))

homog1_start = np.array([
    [1., 0., 0., 1.],
    [0., 1., 0., 0.],
    [0., 0., 1., -1.],
    [0., 0., 0., 1.],
])
homog1_end = np.array([
    [1., 0., 0., 1.],
    [0., 1., 0., 0.],
    [0., 0., 1., -2.],
    [0., 0., 0., 1.],
])

homog2_start = np.array([
    [1., 0., 0., -2.],
    [0., 1., 0., 0.],
    [0., 0., 1., 0.],
    [0., 0., 0., 1.],
])
homog2_end = np.array([
    [1., 0., 0., 2.],
    [0., 1., 0., 0.],
    [0., 0., 1., 1.5],
    [0., 0., 0., 1.],
])

bvh1 = create_fcl_bvh(mesh1)
bvh2 = create_fcl_bvh(mesh2)

obj1_start = fcl.CollisionObject(bvh1, create_fcl_transform(homog1_start))
obj1_end = fcl.CollisionObject(bvh1, create_fcl_transform(homog1_end))
obj2_start = fcl.CollisionObject(bvh2, create_fcl_transform(homog2_start))
obj2_end = fcl.CollisionObject(bvh2, create_fcl_transform(homog2_end))

check_collision(obj1_start, obj2_start)
check_collision(obj2_end, obj2_end)

request = fcl.ContinuousCollisionRequest(
    num_max_iterations=100, gjk_solver_type=fcl.GJKSolverType.GST_INDEP)
result = fcl.ContinuousCollisionResult()
ret = fcl.continuousCollide(obj1_start, create_fcl_transform(homog1_end), obj2_start,
                            create_fcl_transform(homog2_end), request, result)
print_continuous_collision_result("obj1", "obj2", result)

Output:

Collision between obj1 and obj2:
------------------------------
Collision?: False
Number of contacts: 0

Collision between obj1 and obj2:
------------------------------
Collision?: True
Number of contacts: 1

Continuous collision between obj1 and obj2:
------------------------------
Collision?: False
Time of collision: 1.0

I would expect that if collision is True for any of the fcl.collide calls, that fcl.continuousCollide would detect a collision. This doesn't seem to be true.

[weshoke]

FYI, the fix: flexible-collision-library/fcl#550