def place_finger(obj,d,z,normal,distal,hand_normal):
    """
    Function to generate Geometry3D.ConvexPolygon definitions of fingers (including finger geometry and pose)

    Parameters
    ----------
    obj: dict
        Dictionary including information about each surface on the object in the following form:
                     Geometry3D ConvexPolygon  , Geometry3D Vector    , Geometry3D ConvexPolygon
        {surface_no:(surface_polygon_definition, surface_normal_vector, goal_region_polygon(if available))}
    d: float
        Parameter d for given finger
    z: float
        Elevation z for given finger
    normal: Geometry3D.Vector
        Normal vector pointing out of the finger (backhand)
    distal: Geometry3D.Vector
        Distal vector pointing out of the finger (from palm to fingertip)
    hand_normal: Geometry3D.Vector
        Vector normal to the manipulation plane of the hand

    Returns
    ----------
    finger: Geometry3D.ConvexPolygon
        Finger geometry defined as a convex polygon - including pose information (position and orientation)
    """
    # Loop through the object surfaces to find the one that has the same (similar) normal vector with the finger
    for surf in obj:
        if obj[surf][1].angle(normal)<1e-3:
            # Contact surface found
            surface = obj[surf][0]
            break
    
    # Find object length as the distance between two corners of the surface in the distal direction
    for point in surface.points:
        for other_point in surface.points:
            if point==other_point:
                continue
            else:
                edge = Vector(point,other_point)
                angle = edge.angle(distal)
                if angle <1e-3:
                    obj_length = edge.length()
        
    # Find finger center by translating the surface center along z and distal direction using given finger parameters
    finger_center = translate_point(surface.center_point,(hand_normal*z - distal*(d+obj_length/2-finger_w/2)))

    # Find corner 1-4 of the finger by translating the finger center according to given finger parameters
    finger_p1 = translate_point(finger_center,(distal*(finger_w/2)+distal.cross(normal)*(finger_h/2)))
    finger_p2 = translate_point(finger_center,(-distal*(finger_w/2)+distal.cross(normal)*(finger_h/2)))
    finger_p3 = translate_point(finger_center,(-distal*(finger_w/2)-distal.cross(normal)*(finger_h/2)))
    finger_p4 = translate_point(finger_center,(distal*(finger_w/2)-distal.cross(normal)*(finger_h/2)))
    
    # Define Geometry3D.ConvexPolygon for the finger using finger corners
    finger = ConvexPolygon((finger_p1,finger_p2,finger_p3,finger_p4))
    return finger
def find_contact_center(finger_poly,normal,distal,surface,d,z,obj_l):
    """
    Given the finger polygon, corresponding vectors, finger and object parameters, determines the pivoting center for the finger

    Parameters
    ----------
    finger_poly: Geometry3D.ConvexPolygon
        Finger polygon
    normal: Geometry3D.Vector
        Finger normal
    distal: Geometry3D.Vector
        Finger distal vector
    surface: Geometry3D.ConvexPolygon
        Polygon corresponding to the contact surface
    d: float
        Finger parameter d
    z: float
        Finger parameter z
    obj_l: float
        Object length along distal direction

    Returns
    ----------
    center: Geometry3D.Point
        Pivoting center
    """
    # Check if the object exceeds the tip of the finger
    if finger_w - d - obj_l > 0:
        # if not no translation is necessary along distal
        v1 = distal*0
    else:
        v1 = (-(finger_w-d)/2 + (obj_l/2))*distal
    
    # Determine the translation along the hand normal
    v_temp = Vector(surface.center_point,finger_poly.center_point)
    hand_normal = distal.cross(normal)
    if v_temp.length() == 0:
        b = 0
    else:
        ang = v_temp.angle(hand_normal)
        if ang<np.pi/2:
            b = v_temp.length()*abs(np.cos(ang))
        else:
            b = -v_temp.length()*abs(np.cos(ang))
    v2 = b*hand_normal
    # Generate the pivoting center point by translating the surface center using the computed translation vectors
    center = translate_point(surface.center_point,v1+v2)
    return center
def get_finger_param(finger_poly,obj):
    """
    Given the finger polygon and the object find the state parameters (finger parameters)

    Parameters
    ----------
    finger_poly: (surf, normal, distal): tuple
        Finger polygon (Geometry3D.ConvexPolygon), finger normal(Geometry3D.Vector), distal vector (Geometry3D.Vector)
    obj:  dict
        Dictionary including information about each surface on the object in the following form:
                     Geometry3D ConvexPolygon  , Geometry3D Vector    , Geometry3D ConvexPolygon
        {surface_no:(surface_polygon_definition, surface_normal_vector, goal_region_polygon(if available))}
    
    Returns
    ----------
    d: float
        Distance between finger joint and object start
    z: float
        Elevation of the finger on the object
    """
    # Find finger center point
    finger_center = finger_poly[0].center_point
    # Find contact surface
    for surf in obj:
        if obj[surf][1].angle(finger_poly[1])<1e-3:
            surface = obj[surf][0]
            break
    # Find object length alond finger distal vector
    for point in surface.points:
        for other_point in surface.points:
            if point==other_point:
                continue
            else:
                edge = Vector(point,other_point)
                angle = edge.angle(finger_poly[2])
                if angle <1e-3:
                    obj_length = edge.length()
    # Find the center of the contact surface
    obj_center = surface.center_point
    # Center point on the proximal object edge
    close_edge = translate_point(obj_center,(-finger_poly[2]*(obj_length/2)))
    # Generate the vector connecting the proximal edge center and finger center
    vec = Vector(finger_center,close_edge)
    # If the vector has a length of 0, centers align, thus elevation is the same and d is half of the finger length
    if vec.length()==0:
        d = finger_w/2
        z = 0
    # Else compute the angle between distal vector and the generated vector and find the distance components to determine d and z
    else:
        ang = vec.angle(finger_poly[2])
        if ang<np.pi/2:
            b = vec.length()*abs(np.cos(ang))
        else:
            b = -vec.length()*abs(np.cos(ang))
        d = np.round(b+finger_w/2,decimals=1)
        q = obj_center.distance(finger_center)**2-(b+obj_length/2)**2
        if q < 0 and abs(q) < 1e-3:
            q = 0
        if finger_poly[2].cross(finger_poly[1]).angle(vec)<np.pi/2:
            z = -np.round(np.sqrt(q),decimals=1)
        else:
            z = np.round(np.sqrt(q),decimals=1)

    # debugging material
    if np.isnan(z):
        print(obj_center.distance(finger_center))
        print((b+obj_length/2))
        print(obj_center)
        print(finger_center)
        print(b)
        print(vec)
        print(finger_poly[2])
        
        return None

    return d,z