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