Beispiel #1
0
def _measure(p1, p2, p3):
    """ Return hypotenuse, ratio and theta for a trio of ordered points.
    """
    ha, hb = _hypot(p3.x - p1.x, p3.y - p1.y), _hypot(p2.x - p1.x, p2.y - p1.y)
    va, vb = Vector(p1, p2), Vector(p1, p3)
    
    theta = _atan2(va.y, va.x)
    
    x = vb.x * _cos(-theta) - vb.y * _sin(-theta)
    y = vb.x * _sin(-theta) + vb.y * _cos(-theta)
    
    ratio = ha / hb
    theta = _atan2(y, x)
    
    return hb, ratio, theta
Beispiel #2
0
def bset_get_vertex_coordinates(bset_data, scale=1):
    """
    Given a basic set return the list of vertices at the corners.

    :param bset_data: The basic set to get the vertices from
    """

    # Get the vertices.
    vertices = []
    bset_data.compute_vertices().foreach_vertex(vertices.append)
    f = lambda x: _vertex_get_coordinates(x, scale)
    vertices = list(map(f, vertices))

    if len(vertices) <= 1:
        return vertices

    # Sort the vertices in clockwise order.
    #
    # We select a 'center' point that lies within the convex hull of the
    # vertices. We then sort all points according to the direction (given as an
    # angle in radiens) they lie in respect to the center point.
    from math import atan2 as _atan2
    center = ((vertices[0][0] + vertices[1][0]) / 2.0,
              (vertices[0][1] + vertices[1][1]) / 2.0)
    f = lambda x: _atan2(x[0] - center[0], x[1] - center[1])
    vertices = sorted(vertices, key=f)
    return vertices
Beispiel #3
0
def bset_get_vertex_coordinates(bset_data, scale=1):
    """
    Given a basic set return the list of vertices at the corners.

    :param bset_data: The basic set to get the vertices from
    """

    # Get the vertices.
    vertices = []
    bset_data.compute_vertices().foreach_vertex(vertices.append)
    f = lambda x: _vertex_get_coordinates(x, scale)
    vertices = list(map(f, vertices))

    if len(vertices) <= 1:
        return vertices

    # Sort the vertices in clockwise order.
    #
    # We select a 'center' point that lies within the convex hull of the
    # vertices. We then sort all points according to the direction (given as an
    # angle in radiens) they lie in respect to the center point.
    from math import atan2 as _atan2
    center = ((vertices[0][0] + vertices[1][0]) / 2.0,
              (vertices[0][1] + vertices[1][1]) / 2.0)
    f = lambda x: _atan2(x[0]-center[0], x[1]-center[1])
    vertices = sorted(vertices, key=f)
    return vertices
Beispiel #4
0
def euler_angles(Q):
    w, (x, y, z) = Q
    phi   = _atan2(2.*(w*x+y*z), 1.-2.*(x*x+y*y))
    theta = _asin(2*(w*y-x*z))
    psi   = _atan2(2.*(w*z+x*y), 1.-2.*(y*y+z*z))
    return phi, theta, psi
Beispiel #5
0
def theta_u(Q):
    w, v = Q
    norm = _v.norm(v)
    theta = 2. * _atan2(norm, w)
    u = _v.mul(1. / norm, v)
    return theta, u
Beispiel #6
0
def atan2(y, x):
    return _atan2(y, x) if common._use_radians else _deg(_atan2(x))
Beispiel #7
0
    def _phi(self, k_y, k_z):
        """Azimuthal angle.

        Part of coordinate transformation from k-space to (theta, phi)-space.
        """
        return _atan2(k_y, -k_z)
Beispiel #8
0
def atan2(y, x):
    return _atan2(y, x) if common._use_radians else _deg(_atan2(x))
Beispiel #9
0
def Phi(py, px):
    """Calculate phi of a particle."""
    phi = _atan2(py, px)
    if( phi < 0 ):
        phi += 2 * _pi
    return phi
Beispiel #10
0
def cinematicaInversa(state):
    global x
    global y
    global z

    global tilt_x
    global tilt_y
    global tilt_z

    global _last_x
    global _last_y
    global _last_z

    global joint_angles

    #print("x: " + str(x) + "  y: " + str(y) + "  z: " + str(z))

    if (x < 0):
        print("erro: posicao (" + str([x, y, z]) +
              ") fora do alcance do braco robotico")

        #return last valid position
        x = _last_x
        y = _last_y
        z = _last_z
        return joint_angles, False

    #a error is given when the desired position is invalid
    try:
        #main calculations
        x_const = x - _D0
        x_const_pow = x_const * x_const

        y_const = y
        y_const_pow = y_const * y_const

        base_dist = _sqrt(x_const_pow + y_const_pow)
        d = _sqrt(x_const_pow + y_const_pow - _Probe_lenght_pow)

        d_const = d
        z_const = z - _L3

        l_const_pow = d_const * d_const + z_const * z_const
        l_const = _sqrt(l_const_pow)

        t1_const = _acos(
            (_L1_pow - _L2_pow + l_const_pow) / (2 * _L1 * l_const))
        t2_const = _acos((_L1_pow + _L2_pow - l_const_pow) / (2 * _L1 * _L2))
        t3_const = _pi - t1_const - t2_const

        s1 = _acos(z_const / l_const)
        s2 = _acos(d_const / l_const)

        #joint angles
        if (state & (1 << defs.ROBOT_CLOCKWISE)):
            joint_angles[0] = -_pi / 2 - _acos(
                _Probe_lenght / base_dist) + _atan2(
                    y_const, x_const)  #y = 71, x_const <= 0, x <= D0
            joint_angles[1] = -_pi / 2 + (t1_const + s2)
            joint_angles[2] = -_pi + t2_const
            joint_angles[3] = t3_const + s1
            joint_angles[4] = -joint_angles[0] - _pi
        else:
            joint_angles[0] = _acos(_Probe_lenght / base_dist) + _atan2(
                y_const, x_const)
            joint_angles[1] = _pi / 2 - (t1_const + s2)
            joint_angles[2] = _pi - t2_const
            joint_angles[3] = -t3_const - s1
            joint_angles[4] = -joint_angles[0]

        if (state & (1 << defs.IN_STAIR)
                and not state & (1 << defs.HOKUYO_READING)):
            joint_angles[3] += IN_STAIRS_DISPLACEMENT

        #check if t0 and t4 angles exceeds 2*_pi
        if (joint_angles[0] > 2 * _pi):
            joint_angles[0] -= 2 * _pi
        elif (joint_angles[0] < -2 * _pi):
            joint_angles[0] += 2 * _pi
        # if(joint_angles[4] > 2*_pi):
        #     joint_angles[4] -= 2*_pi
        # elif(joint_angles[4] < -2*_pi):
        #     joint_angles[4] += 2*_pi

        joint_angles[0] += tilt_z
        joint_angles[1] += tilt_x
        joint_angles[5] = tilt_y

        #configure tilt
        if ((state & (1 << defs.ROBOT_ON_THE_LEFT))
                and (state & (1 << defs.ROBOT_ANTICLOCKWISE))):
            if (joint_angles[0] < 0):
                joint_angles[0] += 2 * _pi

            joint_angles[0] -= _pi
            # print("################## 1 : joint: " + str(joint_angles[0]) + ",    tilt: " + str(tilt_z))

        elif ((not (state & (1 << defs.ROBOT_ON_THE_LEFT)))
              and (state & (1 << defs.ROBOT_CLOCKWISE))):
            joint_angles[0] += _pi
            # print("################## 2 : joint: " + str(joint_angles[0]) + ",    tilt: " + str(tilt_z))

        # print the joint angles and the x,y,z position of the arm (debuging)
        #print(joint_angles)
        #print("x: " + str(x) + ",  y: " + str(y) + ",  z: " + str(z))
        _last_x = x
        _last_y = y
        _last_z = z
        return joint_angles, True

    except:  #Exception as error: #(for debug, uncomment this part of code)
        #print(error)
        print("erro: posicao (", str([x, y, z]),
              ") fora do alcance do braco robotico")

        #return last valid position
        x = _last_x
        y = _last_y
        z = _last_z
        return joint_angles, False
def labelLine(line,x,label=None,align=True,**kwargs):
    """Places a label on a line plot and orients it to run parallel with the line.

    Given a matplotlib Line instance and x-coordinate, places `label` at the x-coord
    on the given line and orientates it parallel to the line. 

    Author: http://stackoverflow.com/questions/16992038/inline-labels-in-matplotlib

    Arguments:
        line: Matplotlib Line instance
        x: x-coordinate on the line at which the label will be positioned.
        label (str): The label to be added to the line.
        align (bool): whether or not to align the label parallel to the line

    """

    ax = line.get_axes()
    xdata = line.get_xdata()
    ydata = line.get_ydata()

    if (x < xdata[0]) or (x > xdata[-1]):
        print('x label location is outside data range!')
        return

    #Find corresponding y co-ordinate and angle of the
    ip = 1
    for i in range(len(xdata)):
        if x < xdata[i]:
            ip = i
            break

    y = ydata[ip-1] + (ydata[ip]-ydata[ip-1])*(x-xdata[ip-1])/(xdata[ip]-xdata[ip-1])

    if not label:
        label = line.get_label()

    if align:
        #Compute the slope
        dx = xdata[ip] - xdata[ip-1]
        dy = ydata[ip] - ydata[ip-1]
        ang = _degrees(_atan2(dy,dx))

        #Transform to screen co-ordinates
        pt = _np.array([x,y]).reshape((1,2))
        trans_angle = ax.transData.transform_angles(_np.array((ang,)),pt)[0]

    else:
        trans_angle = 0

    #Set a bunch of keyword arguments
    if 'color' not in kwargs:
        kwargs['color'] = line.get_color()

    if ('horizontalalignment' not in kwargs) and ('ha' not in kwargs):
        kwargs['ha'] = 'center'

    if ('verticalalignment' not in kwargs) and ('va' not in kwargs):
        kwargs['va'] = 'center'

    if 'backgroundcolor' not in kwargs:
        kwargs['backgroundcolor'] = ax.get_axis_bgcolor()

    if 'clip_on' not in kwargs:
        kwargs['clip_on'] = True

    if 'zorder' not in kwargs:
        kwargs['zorder'] = 2.5

    ax.text(x,y,label,rotation=trans_angle,**kwargs)
Beispiel #12
0
def blobs2features(blobs, min_hypot=0, min_theta=-pi, max_theta=pi, min_ratio=0, max_ratio=1):
    """ Generate a stream of features conforming to limits.
    
        Yields 5-element tuples: indexes for three blobs followed by feature ratio, theta.
    """
    count = len(blobs)
    
    # one-dimensional arrays of simple positions
    xs = _array([blob.x for blob in blobs], dtype=float)
    ys = _array([blob.y for blob in blobs], dtype=float)
    
    #
    # two-dimensional arrays of component distances between each blob
    #   dx = b.x - a.x, dy = b.y - a.y
    #
    xs_ = repeat(reshape(xs, (1, count)), count, 0)
    ys_ = repeat(reshape(ys, (1, count)), count, 0)
    dxs, dys = transpose(xs_) - xs_, transpose(ys_) - ys_
    
    #
    # two-dimensional array of distances between each blob
    #   distance = sqrt(dx^2 + dy^2)
    #
    distances = nsqrt(dxs ** 2 + dys ** 2)
    
    #
    # Make a list of eligible eligible blob pairs
    #
    hypoteni = distances.copy()
    hypoteni[distances < min_hypot] = 0
    
    hypot_nonzero = nonzero(hypoteni)

    ## Prepend separation distance, longest-to-shortest
    #blobs_sorted = [(distances[i,j], i, j) for (i, j) in zip(*hypot_nonzero)]
    
    # Prepend combined pixel size, largest-to-smallest
    blobs_sorted = [(blobs[i].size + blobs[j].size, i, j) for (i, j) in zip(*hypot_nonzero)]

    blobs_sorted.sort(reverse=True)
    
    #
    # check each hypotenuse for an eligible third point
    #
    for (row, (sort_value, i, j)) in enumerate(blobs_sorted):
        #
        # vector theta for hypotenuse (i, j)
        #
        ij_theta = _atan2(dys[i,j], dxs[i,j])
        
        #
        # rotate each blob[k] around blob[i] by -theta, to get a hypotenuse-relative theta for (i, k)
        #
        ik_xs = dxs[i,:] * _cos(-ij_theta) - dys[i,:] * _sin(-ij_theta)
        ik_ys = dxs[i,:] * _sin(-ij_theta) + dys[i,:] * _cos(-ij_theta)
        
        ik_thetas = arctan2(ik_ys, ik_xs)

        ik_thetas = [(blobs[k].size, k, theta) for (k, theta) in enumerate(ik_thetas)]
        ik_thetas.sort(reverse=True)
        
        #
        # check each blob[k] for correct distance ratio
        #
        for (size, k, theta) in ik_thetas:
            ratio = distances[i,k] / distances[i,j]
            
            if theta < min_theta or max_theta < theta:
                continue

            if ratio < min_ratio or max_ratio < ratio:
                continue
            
            if i == j or i == k or j == k:
                continue
            
            yield (i, j, k, ratio, theta)
Beispiel #13
0
    #
    # Set up some fake blobs.
    #
    blobs = [Point(1, 1), Point(2, 5), Point(3, 2)]
    count = len(blobs)
    
    a, b, c = blobs[0], blobs[1], blobs[2]
    
    dab = _sqrt((b.x - a.x) ** 2 + (b.y - a.y) ** 2)
    dbc = _sqrt((c.x - b.x) ** 2 + (c.y - b.y) ** 2)
    dac = _sqrt((c.x - a.x) ** 2 + (c.y - a.y) ** 2)
    
    ratios = {(0, 1, 2): dac/dab, (1, 0, 2): dbc/dab, (2, 1, 0): dac/dbc}
    
    tab = _atan2(b.y - a.y, b.x - a.x)
    tba = _atan2(a.y - b.y, a.x - b.x)
    tac = _atan2(c.y - a.y, c.x - a.x)
    tca = _atan2(a.y - c.y, a.x - c.x)
    tbc = _atan2(c.y - b.y, c.x - b.x)
    tcb = _atan2(b.y - c.y, b.x - c.x)
    
    thetas = {
        (0, 1, 2): _atan2((c.x - a.x) * _sin(-tab) + (c.y - a.y) * _cos(-tab),
                          (c.x - a.x) * _cos(-tab) - (c.y - a.y) * _sin(-tab)),
        
        (1, 0, 2): _atan2((c.x - b.x) * _sin(-tba) + (c.y - b.y) * _cos(-tba),
                          (c.x - b.x) * _cos(-tba) - (c.y - b.y) * _sin(-tba)),
        
        (0, 2, 1): _atan2((b.x - a.x) * _sin(-tac) + (b.y - a.y) * _cos(-tac),
                          (b.x - a.x) * _cos(-tac) - (b.y - a.y) * _sin(-tac)),
Beispiel #14
0
 def Phi(self):
     """Return Phi of the particle."""
     return _atan2(self._y, self._x)
Beispiel #15
0
def theta_u(Q):
	w, v = Q
	norm = _v.norm(v)
	theta = 2.*_atan2(norm, w)
	u = _v.mul(1./norm, v)
	return theta, u
Beispiel #16
0
def labelLine(line,x,label=None,align=True,**kwargs):
    """Places a label on a line plot and orients it to run parallel with the line.

    Given a matplotlib Line instance and x-coordinate, places `label` at the x-coord
    on the given line and orientates it parallel to the line. 

    Author: http://stackoverflow.com/questions/16992038/inline-labels-in-matplotlib

    Arguments:
        line: Matplotlib Line instance
        x: x-coordinate on the line at which the label will be positioned.
        label (str): The label to be added to the line.
        align (bool): whether or not to align the label parallel to the line

    """

    ax = line.get_axes()
    xdata = line.get_xdata()
    ydata = line.get_ydata()

    if (x < xdata[0]) or (x > xdata[-1]):
        print('x label location is outside data range!')
        return

    #Find corresponding y co-ordinate and angle of the
    ip = 1
    for i in range(len(xdata)):
        if x < xdata[i]:
            ip = i
            break

    y = ydata[ip-1] + (ydata[ip]-ydata[ip-1])*(x-xdata[ip-1])/(xdata[ip]-xdata[ip-1])

    if not label:
        label = line.get_label()

    if align:
        #Compute the slope
        dx = xdata[ip] - xdata[ip-1]
        dy = ydata[ip] - ydata[ip-1]
        ang = _degrees(_atan2(dy,dx))

        #Transform to screen co-ordinates
        pt = _np.array([x,y]).reshape((1,2))
        trans_angle = ax.transData.transform_angles(_np.array((ang,)),pt)[0]

    else:
        trans_angle = 0

    #Set a bunch of keyword arguments
    if 'color' not in kwargs:
        kwargs['color'] = line.get_color()

    if ('horizontalalignment' not in kwargs) and ('ha' not in kwargs):
        kwargs['ha'] = 'center'

    if ('verticalalignment' not in kwargs) and ('va' not in kwargs):
        kwargs['va'] = 'center'

    if 'backgroundcolor' not in kwargs:
        kwargs['backgroundcolor'] = ax.get_axis_bgcolor()

    if 'clip_on' not in kwargs:
        kwargs['clip_on'] = True

    if 'zorder' not in kwargs:
        kwargs['zorder'] = 2.5

    ax.text(x,y,label,rotation=trans_angle,**kwargs)