def rayhit_closet(pfrom, pto, objcm): """ :param pfrom: :param pto: :param objcm: :return: author: weiwei date: 20190805 """ tgt_cdmesh = gen_cdmesh_vvnf(*objcm.extract_rotated_vvnf()) ray = OdeRayGeom(length=1) length, dir = rm.unit_vector(pto - pfrom, toggle_length=True) ray.set(pfrom[0], pfrom[1], pfrom[2], dir[0], dir[1], dir[2]) ray.setLength(length) contact_entry = OdeUtil.collide(ray, tgt_cdmesh, max_contacts=10) contact_points = [ da.pdv3_to_npv3(point) for point in contact_entry.getContactPoints() ] min_id = np.argmin(np.linalg.norm(pfrom - np.array(contact_points), axis=1)) contact_normals = [ da.pdv3_to_npv3(contact_entry.getContactGeom(i).getNormal()) for i in range(contact_entry.getNumContacts()) ] return contact_points[min_id], contact_normals[min_id]
def rayhit_all(pfrom, pto, objcm): """ :param pfrom: :param pto: :param objcm: :return: author: weiwei date: 20190805 """ tgt_cdmesh = gen_cdmesh_vvnf(*objcm.extract_rotated_vvnf()) ray = OdeRayGeom(length=1) length, dir = rm.unit_vector(pto-pfrom, toggle_length=True) ray.set(pfrom[0], pfrom[1], pfrom[2], dir[0], dir[1], dir[2]) ray.setLength(length) hit_entry = OdeUtil.collide(ray, tgt_cdmesh) hit_points = [da.pdv3_to_npv3(point) for point in hit_entry.getContactPoints()] hit_normals = [da.pdv3_to_npv3(hit_entry.getContactGeom(i).getNormal()) for i in range(hit_entry.getNumContacts())] return hit_points, hit_normals