Beispiel #1
0
def find_crossing_faces(vector, vector_pos, vertices_faces, planes):
    crossing_faces = {}
    for face in vertices_faces:
        temp = array(
            polygon_pluecker_test(
                vertices_faces[face], vector_pos, vector / norm(vector)))
        #        print 'pluecker test', face, temp
        crossing_face = False
        if all(temp > 0.) or all(temp < 0.):
            #            print 'Finger will cross face:', face
            crossing_face = True
        elif all(temp >= 0.) or all(temp <= 0.):
            #            print 'not positive'
            # if the zeros are only two and are next to each other then it
            # crosses the face
            i = 0
            j = 0
            while i < len(temp) - j:
                num = temp[i]
                #                print 'num', num
                if num == 0. and not crossing_face:
                    #                    print 'zero'
                    crossing_face = True  # we suppose beforehand
                    i += 1
                    if i == len(temp):
                        num = temp[0]
                    else:
                        num = temp[i]
                    i += 1
                    if num == 0.:
                        # print 'crossing face, two zeros'
                        pass
                    else:
                        # print 'crossing face, one zero'
                        if i == 2:
                            # print 'Don't check for last item'
                            j = 1
                elif num == 0. and crossing_face:
                    crossing_face = False  # we rectify wrong previews assumption # noqa
                    break
                else:
                    i += 1
        if crossing_face:
            # calculate where is crossing the face
            vertices = vertices_faces[face]
            # intersection=triangle_ray_intersection(vertices[0],vertices[1],
            #                                       vertices[2],vector_pos,vector)
            # intersection=plane_ray_intersection(array(planes[face][0]),
            #                                    array(planes[face][1]),
            #                                    vector_pos,vector)
            intersection = plane_ray_intersec2(
                array(planes[face][0]), array(planes[face][1]), vector,
                vector_pos)
            intersection[0] *= norm(vector)
            print('intersection', intersection)
            crossing_faces[face] = [vertices_faces[face], temp, intersection]
    print('crossing_faces', crossing_faces)
    return (crossing_faces)
Beispiel #2
0
def find_finger_object_face(vfinger,pos_finger,vertices_faces, box_planes):
    crossing_faces={}
    for face in vertices_faces:
        temp=array(polygon_pluecker_test(vertices_faces[face],
                                        pos_finger,vfinger/norm(vfinger)))
#        print "pluecker test", face, temp
        crossing_face=False
        if all(temp>0.) or all(temp<0.):
#            print "Finger will cross face:", face
            crossing_face=True
        elif all(temp>=0.) or all(temp<=0.):
#            print "not positive"
            #if the zeros are only two and are next to each other then it
            #crosses the face
            i=0
            j=0
            while i<len(temp)-j:
                num=temp[i]
#                print "num", num
                if num==0. and not crossing_face:
#                    print "zero"
                    crossing_face=True #we suppose beforehand
                    i+=1
                    if i==len(temp):
                        num=temp[0]
                    else:
                        num=temp[i]
                    i+=1
                    if num==0.:
 #                       print "crossing face, two zeros"
                         pass
                    else:
 #                       print "crossing face, one zero"
                        if i==2:
 #                           print "Don't check for last item"
                            j=1
                elif num==0. and crossing_face:
                    crossing_face=False #we rectify wrong previews assumption
                    break
                else:
                    i+=1
        if crossing_face:
            #calculate where is crossing the face
            vertices=vertices_faces[face]
            #intersection=triangle_ray_intersection(vertices[0],vertices[1],
            #                                       vertices[2],pos_finger,vfinger)
            #intersection=plane_ray_intersection(array(box_planes[face][0]),
            #                                    array(box_planes[face][1]),
            #                                    pos_finger,vfinger)
            intersection=plane_ray_intersec2( array(box_planes[face][0]),
                                              array(box_planes[face][1]),
                                              vfinger,pos_finger)
            intersection[0]*=norm(vfinger)
            print "intersection", intersection
            crossing_faces[face]=[vertices_faces[face],temp,intersection]
    print "crossing_faces", crossing_faces
    first=True
    dist_min=None
    for face in crossing_faces:
        if first:
            dist_min=[face,crossing_faces[face][2][0]]
            first=False
        else:
            if abs(crossing_faces[face][2][0]) < abs(dist_min[1]):
                dist_min=[face,crossing_faces[face][2][0]]
    print "dist min", dist_min
    if dist_min and dist_min[1]<=0. :
        for face in crossing_faces:
            if face!=dist_min[0]:
                if crossing_faces[face][2][0]>=0.:
                    return(True,dist_min[0])
    return(False,None)