예제 #1
0
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]
예제 #2
0
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