Python math.atan2() Examples

The following are code examples for showing how to use math.atan2(). They are from open source Python projects. You can vote up the examples you like or vote down the ones you don't like.

Example 1
Project: L.E.S.M.A   Author: NatanaelAntonioli   File: L.E.S.M.A. - Fabrica de Noobs Speedtest.py    Apache License 2.0 7 votes vote down vote up
def distance(origin, destination):
	"""Determine distance between 2 sets of [lat,lon] in km"""

	lat1, lon1 = origin
	lat2, lon2 = destination
	radius = 6371  # km

	dlat = math.radians(lat2 - lat1)
	dlon = math.radians(lon2 - lon1)
	a = (math.sin(dlat / 2) * math.sin(dlat / 2) +
		 math.cos(math.radians(lat1)) *
		 math.cos(math.radians(lat2)) * math.sin(dlon / 2) *
		 math.sin(dlon / 2))
	c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
	d = radius * c

	return d 
Example 2
Project: UR5_Controller   Author: tsinghua-rll   File: quaternion.py    MIT License 6 votes vote down vote up
def to_euler(q):
    # rpy
    sinr = 2.0 * (q[0] * q[1] + q[2] * q[3])
    cosr = 1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2])
    roll = math.atan2(sinr, cosr)

    sinp = 2.0 * (q[0] * q[2] - q[3] * q[1])
    if math.fabs(sinp) >= 1.:
        pitch = math.copysign(np.pi / 2., sinp)
    else:
        pitch = math.asin(sinp)

    siny = 2.0 * (q[0] * q[3] + q[1] * q[2])
    cosy = 1.0 - 2.0 * (q[2] * q[2] + q[3] * q[3])
    yaw = math.atan2(siny, cosy)

    return np.asarray((roll, pitch, yaw), np.float32) 
Example 3
Project: building-boundary   Author: Geodan   File: bounding_box.py    MIT License 6 votes vote down vote up
def compute_edge_angles(edges):
    """
    Compute the angles between the edges and the x-axis.

    Parameters
    ----------
    edges : (Mx2x2) array
        The coordinates of the sets of points that define the edges.

    Returns
    -------
    edge_angles : (Mx1) array
        The angles between the edges and the x-axis.
    """
    edges_count = len(edges)
    edge_angles = np.zeros(edges_count)
    for i in range(edges_count):
        edge_x = edges[i][1][0] - edges[i][0][0]
        edge_y = edges[i][1][1] - edges[i][0][1]
        edge_angles[i] = math.atan2(edge_y, edge_x)

    return np.unique(edge_angles) 
Example 4
Project: building-boundary   Author: Geodan   File: footprint.py    MIT License 6 votes vote down vote up
def line_orientations(lines):
    """
    Computes the orientations of the lines.

    Parameters
    ----------
    lines : list of (2x2) array
        The lines defined by the coordinates two points.

    Returns
    -------
    orientations : list of float
        The orientations of the lines in radians from
        0 to pi (east to west counterclockwise)
        0 to -pi (east to west clockwise)
    """
    orientations = []
    for l in lines:
        dx, dy = l[0] - l[1]
        orientation = math.atan2(dy, dx)
        if not any([np.isclose(orientation, o) for o in orientations]):
            orientations.append(orientation)
    return orientations 
Example 5
Project: building-boundary   Author: Geodan   File: segment.py    MIT License 6 votes vote down vote up
def _create_line_segment(self):
        """
        Defines a line segment of the fitted line by creating
        the end points, length and orientation.

        Raises
        ------
        ValueError
            If not enough points exist to create a line segment.
        """
        if len(self.points) == 1:
            raise ValueError('Not enough points to create a line.')
        else:
            start_point = self._point_on_line(self.points[0])
            end_point = self._point_on_line(self.points[-1])
            self.end_points = np.array([start_point, end_point])
            dx, dy = np.diff(self.end_points, axis=0)[0]
            self.length = math.hypot(dx, dy)
            self.orientation = math.atan2(dy, dx) 
Example 6
Project: soccer-matlab   Author: utra-robosoccer   File: minitaur_ball_gym_env.py    BSD 2-Clause "Simplified" License 6 votes vote down vote up
def _get_observation(self):
    world_translation_minitaur, world_rotation_minitaur = (
        self._pybullet_client.getBasePositionAndOrientation(
            self.minitaur.quadruped))
    world_translation_ball, world_rotation_ball = (
        self._pybullet_client.getBasePositionAndOrientation(self._ball_id))
    minitaur_translation_world, minitaur_rotation_world = (
        self._pybullet_client.invertTransform(world_translation_minitaur,
                                              world_rotation_minitaur))
    minitaur_translation_ball, _ = (
        self._pybullet_client.multiplyTransforms(minitaur_translation_world,
                                                 minitaur_rotation_world,
                                                 world_translation_ball,
                                                 world_rotation_ball))
    distance = math.sqrt(minitaur_translation_ball[0]**2 +
                         minitaur_translation_ball[1]**2)
    angle = math.atan2(minitaur_translation_ball[0],
                       minitaur_translation_ball[1])
    self._observation = [angle - math.pi / 2, distance]
    return self._observation 
Example 7
Project: RLBotPack   Author: RLBot   File: Utilities.py    MIT License 6 votes vote down vote up
def boostHungry(agent):
    closestBoost = agent.me
    bestDistance = math.inf
    bestAngle = 0

    for boost in agent.boosts:
        if boost.spawned:
            distance = distance2D(boost.location, agent.me)
            localCoords = toLocal(closestBoost.location, agent.me)
            angle = abs(math.degrees(math.atan2(localCoords[1], localCoords[0])))
            distance +=  angle*5
            distance += distance2D(agent.me.location,agent.ball.location)
            if boost.bigBoost:
                distance *= .5
            if distance < bestDistance:
                bestDistance = distance
                closestBoost = boost
                bestAngle = angle

    agent.renderCalls.append(renderCall(agent.renderer.draw_line_3d, agent.me.location.data, closestBoost.location.data,
                                        agent.renderer.yellow))
    return efficientMover(agent, closestBoost.location, agent.maxSpd,boostHunt=False) 
Example 8
Project: RLBotPack   Author: RLBot   File: Utilities.py    MIT License 6 votes vote down vote up
def ownGoalCheck(agent,targetVec):
    leftPost = Vector([sign(agent.team) * 800, 5100 * sign(agent.team), 200])
    rightPost = Vector([-sign(agent.team) * 800, 5100 * sign(agent.team), 200])
    center = Vector([0, 5100 * sign(agent.team), 200])

    if distance2D(agent.ball.location,center) < distance2D(agent.me.location,center):
        localTarget = toLocal(targetVec,agent.me)
        targetAngle = correctAngle(math.degrees(math.atan2(localTarget[1],localTarget[0])))

        localRP = toLocal(rightPost, agent.me)
        rightPostAngle = correctAngle(math.degrees(math.atan2(localRP[1], localRP[0])))

        localLP = toLocal(leftPost, agent.me)
        leftPostAngle = correctAngle(math.degrees(math.atan2(localLP[1], localLP[0])))

        if leftPostAngle < targetAngle < rightPostAngle:
            if leftPostAngle - targetAngle > rightPostAngle -targetAngle:
                return True,leftPost
            else:
                return True,rightPost

    return False,None 
Example 9
Project: RLBotPack   Author: RLBot   File: Utilities.py    MIT License 6 votes vote down vote up
def turnTowardsPosition(agent,targetPosition,threshold):
    localTarg = toLocal(targetPosition,agent.me)
    localAngle = correctAngle(math.degrees(math.atan2(localTarg[1],localTarg[0])))
    controls = SimpleControllerState()

    if abs(localAngle) > threshold:
        if agent.forward:
            if localAngle > 0:
                controls.steer = 1
            else:
                controls.steer = -1

            controls.handbrake = True
            if agent.currentSpd <300:
                controls.throttle = .5
        else:
            if localAngle > 0:
                controls.steer = -1
            else:
                controls.steer = 1
            controls.handbrake = True
            if agent.currentSpd <300:
                controls.throttle = -.5

    return controls 
Example 10
Project: RLBotPack   Author: RLBot   File: Utilities.py    MIT License 6 votes vote down vote up
def maxSpeedAdjustment(agent,target):
    tar_local = toLocal(target,agent.me)
    angle = abs(correctAngle(math.degrees(math.atan2(tar_local[1],tar_local[0]))))
    dist = distance2D(agent.me.location,target)
    distCorrection = dist/300

    if dist >=2000:
        return maxPossibleSpeed

    if abs(angle) <=3:
        return maxPossibleSpeed



    cost_inc = maxPossibleSpeed/180
    if dist < 1000:
        cost_inc*=2
    #angle = abs(angle) -10
    angle = abs(angle)
    newSpeed = clamp(maxPossibleSpeed,500,maxPossibleSpeed - (angle*cost_inc))
    #print(f"adjusting speed to {newSpeed}")

    return newSpeed 
Example 11
Project: symbolator   Author: kevinpt   File: cairo_backend.py    MIT License 6 votes vote down vote up
def draw_marker(self, name, mp, tp, weight, c):
    if name in self.markers:
      m_shape, ref, orient, units = self.markers[name]

      c.save()
      c.translate(*mp)
      if orient == 'auto':
        angle = math.atan2(tp[1]-mp[1], tp[0]-mp[0])
        c.rotate(angle)
      elif isinstance(orient, int):
        angle = math.radians(orient)
        c.rotate(angle)

      if units == 'stroke':
        c.scale(weight, weight)
        
      c.translate(-ref[0], -ref[1])
      
      self.draw_shape(m_shape)
      c.restore() 
Example 12
Project: Old-school-processing   Author: cianfrocco-lab   File: ImagePanelTools.py    MIT License 6 votes vote down vote up
def OnLeftClick(self, evt):
		if self.button.GetToggle():
			if self.start is not None:
				x = evt.m_x #- self.imagepanel.offset[0]
				y = evt.m_y #- self.imagepanel.offset[1]
				x0, y0 = self.start
				dx, dy = x - x0, y - y0
				self.measurement = {
					'from': self.start,
					'to': (x, y),
					'delta': (dx, dy),
					'magnitude': math.hypot(dx, dy),
					'angle': math.degrees(math.atan2(dy, dx)),
				}
				mevt = MeasurementEvent(self.imagepanel, dict(self.measurement))
				self.imagepanel.GetEventHandler().AddPendingEvent(mevt)
			self.start = self.imagepanel.view2image((evt.m_x, evt.m_y))

	#-------------------- 
Example 13
Project: pyblish-win   Author: pyblish   File: turtle.py    GNU Lesser General Public License v3.0 5 votes vote down vote up
def towards(self, x, y=None):
        """Return the angle of the line from the turtle's position to (x, y).

        Arguments:
        x -- a number   or  a pair/vector of numbers   or   a turtle instance
        y -- a number       None                            None

        call: distance(x, y)         # two coordinates
        --or: distance((x, y))       # a pair (tuple) of coordinates
        --or: distance(vec)          # e.g. as returned by pos()
        --or: distance(mypen)        # where mypen is another turtle

        Return the angle, between the line from turtle-position to position
        specified by x, y and the turtle's start orientation. (Depends on
        modes - "standard" or "logo")

        Example (for a Turtle instance named turtle):
        >>> turtle.pos()
        (10.00, 10.00)
        >>> turtle.towards(0,0)
        225.0
        """
        if y is not None:
            pos = Vec2D(x, y)
        if isinstance(x, Vec2D):
            pos = x
        elif isinstance(x, tuple):
            pos = Vec2D(*x)
        elif isinstance(x, TNavigator):
            pos = x._position
        x, y = pos - self._position
        result = round(math.atan2(y, x)*180.0/math.pi, 10) % 360.0
        result /= self._degreesPerAU
        return (self._angleOffset + self._angleOrient*result) % self._fullcircle 
Example 14
Project: pyblish-win   Author: pyblish   File: turtle.py    GNU Lesser General Public License v3.0 5 votes vote down vote up
def heading(self):
        """ Return the turtle's current heading.

        No arguments.

        Example (for a Turtle instance named turtle):
        >>> turtle.left(67)
        >>> turtle.heading()
        67.0
        """
        x, y = self._orient
        result = round(math.atan2(y, x)*180.0/math.pi, 10) % 360.0
        result /= self._degreesPerAU
        return (self._angleOffset + self._angleOrient*result) % self._fullcircle 
Example 15
Project: pyblish-win   Author: pyblish   File: test_float.py    GNU Lesser General Public License v3.0 5 votes vote down vote up
def test_negative_zero(self):
        def pos_pos():
            return 0.0, math.atan2(0.0, -1)
        def pos_neg():
            return 0.0, math.atan2(-0.0, -1)
        def neg_pos():
            return -0.0, math.atan2(0.0, -1)
        def neg_neg():
            return -0.0, math.atan2(-0.0, -1)
        self.assertEqual(pos_pos(), neg_pos())
        self.assertEqual(pos_neg(), neg_neg()) 
Example 16
Project: pyblish-win   Author: pyblish   File: test_float.py    GNU Lesser General Public License v3.0 5 votes vote down vote up
def test_underflow_sign(self):
        # check that -1e-1000 gives -0.0, not 0.0
        self.assertEqual(math.atan2(-1e-1000, -1), math.atan2(-0.0, -1))
        self.assertEqual(math.atan2(float('-1e-1000'), -1),
                         math.atan2(-0.0, -1)) 
Example 17
Project: UR5_Controller   Author: tsinghua-rll   File: API.py    MIT License 5 votes vote down vote up
def from_q_to_rad_axis(q):
        """
        :param q: quaternion in w-i-j-k format
        :return: rad in format x-y-z
        """
        norm_axis = math.sqrt(q[1] ** 2 + q[2] ** 2 + q[3] ** 2)
        rad = 2.0 * math.atan2(norm_axis, q[0])
        if norm_axis < 1e-5:
            return np.zeros((3,), dtype=np.float32)
        else:
            return rad * np.asarray(q[1:], dtype=np.float32) / norm_axis 
Example 18
Project: UR5_Controller   Author: tsinghua-rll   File: quaternion.py    MIT License 5 votes vote down vote up
def to_angle_axis(q0):
    rad = math.atan2(np.linalg.norm(q0[1:]), q0[0])
    sin_x = math.sin(rad)
    if abs(sin_x) < 1e-5:
        return np.array([sin_x * 2.0, q0[1], q0[2], q0[3]], dtype=np.float32)
    return np.array([rad * 2.0, q0[1] / sin_x, q0[2] / sin_x, q0[3] / sin_x], dtype=np.float32) 
Example 19
Project: mtrl-auto-uav   Author: brunapearson   File: utils.py    MIT License 5 votes vote down vote up
def to_eularian_angles(q):
    z = q.z_val
    y = q.y_val
    x = q.x_val
    w = q.w_val
    ysqr = y * y

    # roll (x-axis rotation)
    t0 = +2.0 * (w*x + y*z)
    t1 = +1.0 - 2.0*(x*x + ysqr)
    roll = math.atan2(t0, t1)

    # pitch (y-axis rotation)
    t2 = +2.0 * (w*y - z*x)
    if (t2 > 1.0):
        t2 = 1
    if (t2 < -1.0):
        t2 = -1.0
    pitch = math.asin(t2)

    # yaw (z-axis rotation)
    t3 = +2.0 * (w*z + x*y)
    t4 = +1.0 - 2.0 * (ysqr + z*z)
    yaw = math.atan2(t3, t4)

    return (pitch, roll, yaw) 
Example 20
Project: Equsant-Self-Balancing-Robot-Python   Author: shivaay1   File: equsant.py    MIT License 5 votes vote down vote up
def y_rotation(x, y, z):
    radians = math.atan2(x, distance(y, z))
    return -math.degrees(radians) 
Example 21
Project: Equsant-Self-Balancing-Robot-Python   Author: shivaay1   File: equsant.py    MIT License 5 votes vote down vote up
def x_rotation(x, y, z):
    radians = math.atan2(y, distance(x, z))
    return math.degrees(radians) 
Example 22
Project: python.math.expression.parser.pymep   Author: sbesada   File: complex.py    Apache License 2.0 5 votes vote down vote up
def __log__(self):
        return Complex(math.log(self.abs(self)), math.atan2(self.imag, self.real))
    

    #E^c 
Example 23
Project: python.math.expression.parser.pymep   Author: sbesada   File: complex.py    Apache License 2.0 5 votes vote down vote up
def __arg__(self):
        angle = math.atan2(self.imag, self.real)
        print (angle)
        if angle < 0.0:
            angle = (2 * math.pi) + angle
        
        return (angle * 180) / math.pi 
Example 24
Project: python.math.expression.parser.pymep   Author: sbesada   File: complex.py    Apache License 2.0 5 votes vote down vote up
def __log10__(self):

        rpart = math.sqrt((self.real * self.real) + (self.imag * self.imag))
        ipart = math.atan2(self.imag,self.real)
        if ipart > math.pi:
            ipart = ipart - (2.0 * math.pi)
        
        return Complex(math.log10(rpart), (1 /math.log(10)) * ipart) 
Example 25
Project: MeteorTracker   Author: heidtn   File: match_events.py    MIT License 5 votes vote down vote up
def distanceBetweenCoords(lat1, lon1, lat2, lon2):
    """
    This uses the haversine formula to calculate the great-circle distance
    between two points.

    Parameters
    ----------
    lat1 : float
        The latitude of the first point
    lon1 : float
        The longitude of the first point
    lat2 : float
        The latitude of the second point
    lon2 : float
        The longitude of the second point
    """
    earthRadius = 6371.0  # earths radius in km
    phi1 = math.radians(lat1)
    phi2 = math.radians(lat2)
    deltaPhi = math.radians(lat2 - lat1)
    deltaLambda = math.radians(lon2 - lon1)

    a = math.sin(deltaPhi/2.0)**2 + \
                 math.cos(phi1)*math.cos(phi2)*(math.sin(deltaLambda/2.0)**2)

    c = 2.0*math.atan2(math.sqrt(a), math.sqrt(1 - a))
    d = earthRadius*c

    return d 
Example 26
Project: RLBotPack   Author: RLBot   File: penguin_bot.py    MIT License 5 votes vote down vote up
def to_rotation(self):
		v = Vector2(self.x, self.y)
		r = math.atan2(v.y, v.x)
		r2 = math.atan(self.z / v.len())
		# r2 = math.pi * 0.3
		return Vector2(r, r2) 
Example 27
Project: RLBotPack   Author: RLBot   File: penguin_bot.py    MIT License 5 votes vote down vote up
def correction(car, ideal):
	v = Make_Vect(ideal).align_from(car.physics.rotation)
	return constrain_pi(math.atan2(-v.y, v.x)) 
Example 28
Project: RLBotPack   Author: RLBot   File: util.py    MIT License 5 votes vote down vote up
def angle(a, b):
    """Returns angle between two 2d points in radians"""
    return math.atan2(a[1] - b[1], a[0] - b[0]) 
Example 29
Project: RLBotPack   Author: RLBot   File: util.py    MIT License 5 votes vote down vote up
def angle2(a, b):
    """Returns angle between two 2d points in radians"""
    return math.atan2(a[0] - b[0], a[1] - b[1]) 
Example 30
Project: RLBotPack   Author: RLBot   File: Util.py    MIT License 5 votes vote down vote up
def spherical(x, y, z, Urot=True):
    d = math.sqrt(x * x + y * y + z * z)
    if d != 0:
        i = math.acos(z / d)
    else:
        i = 0
    a = math.atan2(y, x)
    if Urot:
        return d, Range180(a / pi - .5, 1), Range180(i / pi - .5, 1)
    else:
        return d, a, i
    # https://en.wikipedia.org/wiki/Spherical_coordinate_system 
Example 31
Project: RLBotPack   Author: RLBot   File: util.py    MIT License 5 votes vote down vote up
def angle(a, b):
    """Returns angle between two 2d points in radians"""
    return math.atan2(a[1] - b[1], a[0] - b[0]) 
Example 32
Project: RLBotPack   Author: RLBot   File: util.py    MIT License 5 votes vote down vote up
def angle2(a, b):
    """Returns angle between two 2d points in radians"""
    return math.atan2(a[0] - b[0], a[1] - b[1]) 
Example 33
Project: RLBotPack   Author: RLBot   File: util.py    MIT License 5 votes vote down vote up
def spherical(x, y, z, Urot=True):
    """Converts from cartesian to spherical coordinates."""
    d = math.sqrt(x * x + y * y + z * z)
    if d != 0:
        i = math.acos(z / d)
    else:
        i = 0
    a = math.atan2(x, y)
    if Urot:
        return d, -a / PI, Range180(i - PI / 2, PI) / PI
    else:
        return d, a, i 
Example 34
Project: RLBotPack   Author: RLBot   File: util.py    MIT License 5 votes vote down vote up
def get_speed(agent, location):
    car = agent.info.my_car
    local = dot(location - car.pos, car.theta)
    angle = cap(math.atan2(local[1], local[0]), -3, 3)
    distance = distance_2d(car.pos, location)
    if distance > 2.5 * velocity_2d(car.vel):
        return 2250
    else:
        return 2250 - (400 * (angle ** 2)) 
Example 35
Project: RLBotPack   Author: RLBot   File: util.py    MIT License 5 votes vote down vote up
def can_dodge(agent, target):
    bot_to_target = target - agent.info.my_car.pos
    local_bot_to_target = dot(bot_to_target, agent.info.my_car.theta)
    angle_front_to_target = math.atan2(local_bot_to_target[1], local_bot_to_target[0])
    distance_bot_to_target = norm(vec2(bot_to_target))
    good_angle = math.radians(-10) < angle_front_to_target < math.radians(10)
    on_ground = agent.info.my_car.on_ground and agent.info.my_car.pos[2] < 100
    going_fast = velocity_2d(agent.info.my_car.vel) > 1250
    target_not_in_goal = not agent.info.my_goal.inside(target)
    return good_angle and distance_bot_to_target > 2000 and on_ground and going_fast and target_not_in_goal 
Example 36
Project: RLBotPack   Author: RLBot   File: shooting.py    MIT License 5 votes vote down vote up
def should_dodge(agent):
    car = agent.info.my_car
    their_goal = agent.info.their_goal
    close_to_goal = distance_2d(car.pos, their_goal.center) < 4000
    aiming_for_goal = abs(line_backline_intersect(their_goal.center[1], vec2(car.pos), vec2(car.forward()))) < 850
    bot_to_target = agent.info.ball.pos - car.pos
    local_bot_to_target = dot(bot_to_target, agent.info.my_car.theta)
    angle_front_to_target = math.atan2(local_bot_to_target[1], local_bot_to_target[0])
    close_to_ball = norm(vec2(bot_to_target)) < 850
    good_angle = math.radians(-10) < angle_front_to_target < math.radians(10)
    return close_to_ball and close_to_goal and aiming_for_goal and good_angle 
Example 37
Project: RLBotPack   Author: RLBot   File: Zoomelette.py    MIT License 5 votes vote down vote up
def Controller_output(agent,target,speed):
    Controller = SimpleControllerState()
    LocalTagret = agent.car.matrix.dot(target-agent.car.loc)
    angle_target = math.atan2(LocalTagret[1],LocalTagret[0])
    Controller.steer = steer(angle_target)
    agentSpeed = velocity2D(agent.car)
    Controller.throttle,Controller.boost = throttle(speed,agentSpeed)
    if abs(angle_target) > 2:
        Controller.handbrake = True
    else:
        Controller.handbrake = False
    return Controller 
Example 38
Project: RLBotPack   Author: RLBot   File: States.py    MIT License 5 votes vote down vote up
def update(self):
        controller_state = SimpleControllerState()
        jump = flipHandler(self.agent, self.flip_obj)
        if jump:
            if self.targetCode == 1:
                controller_state.pitch = -1
                controller_state.steer = 0
                controller_state.throttle = 1

            elif self.targetCode == 0:
                ball_local = toLocal(self.agent.ball.location, self.agent.me)
                ball_angle = math.atan2(ball_local.data[1], ball_local.data[0])
                controller_state.jump = True
                controller_state.yaw = math.sin(ball_angle)
                pitch = -math.cos(ball_angle)
                controller_state.pitch = pitch
                if pitch > 0:
                    controller_state.throttle = -1
                else:
                    controller_state.throttle = 1

            elif self.targetCode == 2:
                controller_state.pitch = 0
                controller_state.steer = 0
                controller_state.yaw = 0
            elif self.targetCode == 3:
                controller_state.pitch = 1
                controller_state.steer = 0
                controller_state.throttle = -1

            elif self.targetCode == -1:
                controller_state.pitch = 0
                controller_state.steer = 0
                controller_state.throttle = 0

        controller_state.jump = jump
        controller_state.boost = False
        if self.flip_obj.flipDone:
            self.active = False

        return controller_state 
Example 39
Project: RLBotPack   Author: RLBot   File: Utilities.py    MIT License 5 votes vote down vote up
def correction_to(self, ideal):
        current_in_radians = math.atan2(self[1], -self[0])
        ideal_in_radians = math.atan2(ideal[1], -ideal[0])

        correction = ideal_in_radians - current_in_radians
        if abs(correction) > math.pi:
            if correction < 0:
                correction += 2 * math.pi
            else:
                correction -= 2 * math.pi

        return correction 
Example 40
Project: RLBotPack   Author: RLBot   File: Utilities.py    MIT License 5 votes vote down vote up
def demoMagic(agent):
    currentSpd = agent.getCurrentSpd()
    if currentSpd <1900:
        if agent.me.boostLevel <=0:
            agent.activeState.active = False
    e_goal = Vector([0, 5100 * -sign(agent.team), 200])
    best = None
    distance = math.inf
    for e in agent.enemies:
        if e.location[2] <= 120:
            _distance = distance2D(e_goal,e.location)
            if _distance < distance:
                distance = _distance
                best = e

    if best != None:
        if currentSpd <=100:
            currentSpd = 100

        currentTimeToTarget = distance / currentSpd
        lead = clamp(agent.deltaTime*60,agent.deltaTime*5,agent.deltaTime*distance/500)
        difference = best.velocity.scale(lead)
        targetPos = e.location + difference
        agent.renderCalls.append(
            renderCall(agent.renderer.draw_line_3d, agent.me.location.data, targetPos.data, agent.renderer.purple))

        if currentTimeToTarget <= agent.deltaTime*30:
            targetLocal = toLocal(targetPos,agent.me)
            angle = math.degrees(math.atan2(targetLocal[1],targetLocal[0]))
            if abs(angle) <= 40:
                agent.setJumping(0)

        return testMover(agent,targetPos,2300)



    else:
        return None 
Example 41
Project: RLBotPack   Author: RLBot   File: Utilities.py    MIT License 5 votes vote down vote up
def angle2(target_location,object_location):
    difference = getLocation(target_location) - getLocation(object_location)
    return math.atan2(difference[1], difference[0]) 
Example 42
Project: RLBotPack   Author: RLBot   File: Utilities.py    MIT License 5 votes vote down vote up
def greedyMover(agent,target_object):
    controller_state = SimpleControllerState()
    controller_state.handbrake = False
    location = toLocal(target_object, agent.me)
    angle = math.atan2(location.data[1], location.data[0])
    controller_state.throttle = 1
    if getVelocity(agent.me.velocity) < 2200:
        if agent.onSurface:
            controller_state.boost = True
    controller_state.jump = False

    controller_state.steer = Gsteer(angle)

    return controller_state 
Example 43
Project: RLBotPack   Author: RLBot   File: Utilities.py    MIT License 5 votes vote down vote up
def CB_Reworked(agent,targetVec):
    dist = clamp(25000, 1, distance2D(agent.me.location, targetVec))
    ballDist = clamp(25000, 1, distance2D(agent.me.location, agent.ball.location))
    destinationEstimate = inaccurateArrivalEstimator(agent,targetVec)
    locTarget = toLocal(targetVec, agent.me)
    targetAngle = correctAngle(math.degrees(math.atan2(locTarget[1], locTarget[0])))

    bestBoost = None
    bestAngle = 0
    angleDisparity = 1000
    bestDist = math.inf
    bestEstimate = math.inf
    goodBoosts = []
    for b in agent.boosts:
        _dist = distance2D(b.location, agent.me.location)
        if _dist < dist*.6:
            localCoords = toLocal(b.location, agent.me)
            angle = correctAngle(math.degrees(math.atan2(localCoords[1], localCoords[0])))
            _angleDisparity = targetAngle - angle

            if _angleDisparity > targetAngle-30 and _angleDisparity < targetAngle+30:
                goodBoosts.append(b)

    for b in goodBoosts:
        pathEstimate = inaccurateArrivalEstimator(agent,b.location) + inaccurateArrivalEstimatorRemote(agent,b.location,targetVec)
        if agent.me.boostLevel < 50:
            if b.bigBoost:
                pathEstimate*=.8
        if pathEstimate < bestEstimate:
            bestBoost = b
            bestEstimate = pathEstimate

    if bestEstimate < destinationEstimate*1.15 or bestEstimate < agent.ballDelay:
        return bestBoost.location
    else:
        return None 
Example 44
Project: RLBotPack   Author: RLBot   File: vec.py    MIT License 5 votes vote down vote up
def rotation_to_euler(rotation: Mat33) -> Vec3:
    return Vec3(
        math.atan2(rotation.get(2, 0), norm(Vec3(rotation.get(0, 0), rotation.get(1, 0)))),
        math.atan2(rotation.get(1, 0), rotation.get(0, 0)),
        math.atan2(-rotation.get(2, 1), rotation.get(2, 2))
    ) 
Example 45
Project: RLBotPack   Author: RLBot   File: vector_math.py    MIT License 5 votes vote down vote up
def vec2angle(vec2):
    '''
    >>> vec2angle(Vec2(0, 1))
    1.5707963267948966
    '''
    return math.atan2(vec2[1], vec2[0]) 
Example 46
Project: RLBotPack   Author: RLBot   File: vector_math.py    MIT License 5 votes vote down vote up
def clockwise_angle(a, b):
    '''
    >>> clockwise_angle(Vec2(1, 0), Vec2(0, -1))
    -1.5707963267948966
    '''
    # Note: x=right, y=up
    dot = a.dot(b)      # dot product
    det = np.linalg.det([a, b])      # determinant
    return math.atan2(det, dot)  # atan2(y, x) or atan2(sin, cos) 
Example 47
Project: RLBotPack   Author: RLBot   File: SkyBot.py    MIT License 5 votes vote down vote up
def avoid_enemy_colision(self, values):
        enemy_avoid = False
        for enemy_index in range(self.num_cars):
            if self.bot_team != values.game_cars[enemy_index].team:  # aka enemy
                self.enemy_loc_x = values.game_cars[enemy_index].physics.location.x
                self.enemy_loc_y = values.game_cars[enemy_index].physics.location.y
                self.enemy_loc_z = values.game_cars[enemy_index].physics.location.z
                self.enemy_speed_x = values.game_cars[enemy_index].physics.velocity.x
                self.enemy_speed_y = values.game_cars[enemy_index].physics.velocity.y
                self.enemy_speed_z = values.game_cars[enemy_index].physics.velocity.z
                self.enemy_rot = values.game_cars[enemy_index].physics.rotation
                self.enemy_rot_yaw = abs(self.bot_rot.yaw) % 65536 / 65536 * 360
                if self.enemy_rot.yaw < 0:
                    self.enemy_rot_yaw *= -1

                self.enemy_distance = self.distance(self.enemy_loc_x, self.enemy_loc_y, 0, self.bot_loc_x,
                                                    self.bot_loc_y, 0)
                self.enemy_speed = self.distance(self.enemy_speed_x, self.enemy_speed_y, 0, self.bot_speed_x,
                                                 self.bot_speed_y, 0)

                angle_between_enemy_and_bot = math.degrees(
                    math.atan2(self.bot_loc_y - self.enemy_loc_y, self.bot_loc_y - self.enemy_loc_y))
                self.angle_enemy_to_bot = angle_between_enemy_and_bot - self.enemy_rot_yaw
                # Correct the values
                if self.angle_enemy_to_bot < -180:
                    self.angle_enemy_to_bot += 360
                if self.angle_enemy_to_bot > 180:
                    self.angle_enemy_to_bot -= 360

                if (self.enemy_distance * self.enemy_speed) < 1 and abs(self.angle_enemy_to_bot) < 50:
                    enemy_avoid = True

        if self.bot_state == 'dribbling' and enemy_avoid:
            a = self.goal_to_ball_vector()
            x = a[0]
            y = a[1]
            self.flick(pitch=-y, roll=-x)
            print(6)
        elif enemy_avoid:
            self.controller.jump = True
            # self.jump_time_end=self.game_time+0.3 
Example 48
Project: Parallel.GAMIT   Author: demiangomez   File: pyVoronoi.py    GNU General Public License v3.0 5 votes vote down vote up
def calculate_Vincenty_distance_between_spherical_points(cartesian_array_1,cartesian_array_2,sphere_radius):
    '''Apparently, the special case of the Vincenty formula (http://en.wikipedia.org/wiki/Great-circle_distance) may be the most accurate method for calculating great-circle distances.'''
    spherical_array_1 = convert_cartesian_array_to_spherical_array(cartesian_array_1)
    spherical_array_2 = convert_cartesian_array_to_spherical_array(cartesian_array_2)
    lambda_1 = spherical_array_1[1]
    lambda_2 = spherical_array_2[1]
    phi_1 = spherical_array_1[2]
    phi_2 = spherical_array_2[2]
    delta_lambda = abs(lambda_2 - lambda_1)
    delta_phi = abs(phi_2 - phi_1)
    radian_angle = math.atan2( math.sqrt( (math.sin(phi_2)*math.sin(delta_lambda))**2 + (math.sin(phi_1)*math.cos(phi_2) - math.cos(phi_1)*math.sin(phi_2)*math.cos(delta_lambda)  )**2 ),  (math.cos(phi_1) * math.cos(phi_2) + math.sin(phi_1) * math.sin(phi_2) * math.cos(delta_lambda) ) )
    spherical_distance = sphere_radius * radian_angle
    return spherical_distance 
Example 49
Project: Old-school-processing   Author: cianfrocco-lab   File: ImagePanelTools.py    MIT License 5 votes vote down vote up
def OnMotion(self, evt, dc):
		if self.button.GetToggle():
			if self.left_or_right == 'left':
				x,y = self.imagepanel.view2image((evt.m_x, evt.m_y))
				self.xypath.append((x,y))
				return True
			elif self.left_or_right == 'right':
				if not self.ellipse_params:
					return False
				# calculate distance dragged
				x,y = self.imagepanel.view2image((evt.m_x, evt.m_y))
				dx = x - self.start[0]
				dy = y - self.start[1]
				print ' XY', dx,dy
				# calculate new key point
				newellipseparams = dict(self.start_ellipse_params)
				print 'AXIS', self.ellipsepointaxis
				newx = self.ellipsepoint[0]+dx
				newy = self.ellipsepoint[1]+dy
				if self.ellipsepointaxis == 'center':
					newellipseparams['center'] = newx,newy
				else:
					newvect = newx-newellipseparams['center'][0], newy-newellipseparams['center'][1]
					newdist = math.hypot(*newvect)
					newellipseparams[self.ellipsepointaxis] = newdist
					newangle = math.atan2(*newvect)
					while newangle < 0:
						newangle += 2*math.pi
					print 'pointangle', self.ellipsepointangle
					print 'newangle', newangle
					dangle = newangle - self.ellipsepointangle
					print 'dangle', dangle
					#newellipseparams['alpha'] += dangle
				self.ellipse_params = newellipseparams
				self.ellipse = self.drawEllipse()
				self.imagepanel.UpdateDrawing()
		return False

	#-------------------- 
Example 50
Project: Old-school-processing   Author: cianfrocco-lab   File: ImagePanelTools.py    MIT License 5 votes vote down vote up
def getToolTipStrings(self, x, y, value):
		if self.button.GetToggle() and self.start is not None:
			x0, y0 = self.start
			dx, dy = x - x0, y - y0
			return ['From (%d, %d) x=%d y=%d d=%.2f a=%.0f' % (x0, y0, dx, dy, math.hypot(dx, dy),math.degrees(math.atan2(dy, dx)))]
		else:
			return []

##################################
##
################################## 
Example 51
Project: hepaccelerate   Author: hepaccelerate   File: backend_cuda.py    BSD 3-Clause "New" or "Revised" License 5 votes vote down vote up
def cartesian_to_spherical_devfunc(px, py, pz, e):
    pt = math.sqrt(px**2 + py**2)
    eta = math.asinh(pz / pt)
    phi = math.atan2(py, px)
    mass = math.sqrt(e**2 - px**2 - py**2 - pz**2)
    return pt, eta, phi, mass 
Example 52
Project: hive-presto-docker   Author: dinhnhatbang   File: process_geojson.py    GNU General Public License v3.0 5 votes vote down vote up
def center(geolocations):

    """
    Provide a relatively accurate center lat, lon returned as a list pair, given
    a list of list pairs.
    ex: in: geolocations = ((lat1,lon1), (lat2,lon2),)
        out: (center_lat, center_lon)
    """
    from math import cos, sin, atan2, sqrt, pi
    x = 0
    y = 0
    z = 0

    for lat, lon in geolocations:
        lat = float(lat) * pi / 180
        lon = float(lon) * pi / 180
        x += cos(lat) * cos(lon)
        y += cos(lat) * sin(lon)
        z += sin(lat)


    x = float(x / len(geolocations))
    y = float(y / len(geolocations))
    z = float(z / len(geolocations))

    return (atan2(z, sqrt(x * x + y * y))  * 180 / pi, atan2(y, x) * 180 / pi) 
Example 53
Project: dump1090-tools   Author: mutability   File: adsb-polar.py    ISC License 5 votes vote down vote up
def latlngup_to_relxyz(c,l):
    # this converts WGS84 (lat,lng,alt) to a rotated ECEF frame
    # where the earth center is still at the origin
    # but (clat,clng,calt) has been rotated to lie on the positive X axis

    clat,clng,calt = c
    llat,llng,lalt = l

    # rotate by -clng around Z to put C on the X/Z plane
    # (this is easy to do while still in WGS84 coordinates)
    llng = llng - clng

    # find angle between XY plane and C
    cx,cy,cz = latlngup_to_ecef((clat,0,calt))
    a = math.atan2(cz,cx)
    
    # convert L to (rotated around Z) ECEF
    lx,ly,lz = latlngup_to_ecef((llat,llng,lalt))

    # rotate by -a around Y to put C on the X axis
    asin = math.sin(-a)
    acos = math.cos(-a)
    rx = lx * acos - lz * asin
    rz = lx * asin + lz * acos

    return (rx, ly, rz)

# calculate range, bearing, elevation from C to L 
Example 54
Project: dump1090-tools   Author: mutability   File: adsb-polar.py    ISC License 5 votes vote down vote up
def range_bearing_elevation(c,l):
    # rotate C onto X axis
    crx, cry, crz = latlngup_to_relxyz(c,c)
    # rotate L in the same way
    lrx, lry, lrz = latlngup_to_relxyz(c,l)

    # Now we have cartesian coordinates with C on
    # the X axis with ground plane YZ and north along +Z.

    dx, dy, dz = lrx-crx, lry-cry, lrz-crz
    rng = math.sqrt(dx*dx + dy*dy + dz*dz)
    bearing = (360 + 90 - rtod(math.atan2(dz,dy))) % 360
    elev = rtod(math.asin(dx / rng))

    return (rng, bearing, elev) 
Example 55
Project: dump1090-tools   Author: mutability   File: adsb-polar-2.py    ISC License 5 votes vote down vote up
def latlngup_to_relxyz(c,l):
    # this converts WGS84 (lat,lng,alt) to a rotated ECEF frame
    # where the earth center is still at the origin
    # but (clat,clng,calt) has been rotated to lie on the positive X axis

    clat,clng,calt = c
    llat,llng,lalt = l

    # rotate by -clng around Z to put C on the X/Z plane
    # (this is easy to do while still in WGS84 coordinates)
    llng = llng - clng

    # find angle between XY plane and C
    cx,cy,cz = latlngup_to_ecef((clat,0,calt))
    a = math.atan2(cz,cx)

    # convert L to (rotated around Z) ECEF
    lx,ly,lz = latlngup_to_ecef((llat,llng,lalt))

    # rotate by -a around Y to put C on the X axis
    asin = math.sin(-a)
    acos = math.cos(-a)
    rx = lx * acos - lz * asin
    rz = lx * asin + lz * acos

    return (rx, ly, rz)

# great circle distance from C to L, assuming spherical geometry (~0.3% error)
# from http://www.movable-type.co.uk/scripts/latlong.html ("haversine formula") 
Example 56
Project: dump1090-tools   Author: mutability   File: adsb-polar-2.py    ISC License 5 votes vote down vote up
def gc_distance(c,l):
    # great circle distance (assumes spherical geometry!)
    lat1 = dtor(c[0])
    lat2 = dtor(l[0])
    delta_lat = lat2 - lat1
    delta_lon = dtor(c[1] - l[1])

    a = math.sin(delta_lat/2) * math.sin(delta_lat/2) + math.cos(lat1) * math.cos(lat2) * math.sin(delta_lon/2) * math.sin(delta_lon/2)
    c = 2 * math.atan2(math.sqrt(a), math.sqrt(1-a))
    return MEAN_R * c 
Example 57
Project: dump1090-tools   Author: mutability   File: adsb-polar-2.py    ISC License 5 votes vote down vote up
def range_bearing_elevation_from(c):
    # build a function that calculates ranges, bearing, elevation from C
    clat,clng,calt = c

    # rotate by -clng to put C on the XZ plane
    # find angle between XY plane and C
    cx,cy,cz = latlngup_to_ecef((clat,0,calt))
    a = math.atan2(cz,cx)
    
    # rotate by -a around Y to put C on the X axis
    asin = math.sin(-a)
    acos = math.cos(-a)

    crx = cx * acos - cz * asin
    cry = cy                         # should be zero
    crz = cx * asin + cz * acos      # should be zero

    def rbe(l):
        # rotate L into our new reference frame
        llat,llng,lalt = l
        # rotate by -clng, convert to ECEF
        lx,ly,lz = latlngup_to_ecef((llat,llng - clng,lalt))

        # rotate by -a around Y
        lrx = lx * acos - lz * asin
        lry = ly
        lrz = lx * asin + lz * acos

        # Now we have cartesian coordinates with C on
        # the +X axis, ground plane YZ, north along +Z.

        dx, dy, dz = lrx-crx, lry-cry, lrz-crz
        slant = math.sqrt(dx*dx + dy*dy + dz*dz)             # true line-of-sight range
        bearing = (360 + 90 - rtod(math.atan2(dz,dy))) % 360 # bearing around X axis
        elev = rtod(math.asin(dx / slant))                   # elevation from horizon (YZ plane)
        horiz_range = math.sqrt(dy*dy + dz*dz)               # distance projected onto YZ (ground/horizon plane); something like ground distance if the Earth was flat
        return (slant, horiz_range, bearing, elev, (lrx,lry,lrz))

    return rbe

# calculate true range, bearing, elevation from C to L 
Example 58
Project: quadcopter-simulation   Author: hbd730   File: utils.py    BSD 3-Clause "New" or "Revised" License 5 votes vote down vote up
def RotToRPY(R):
    phi = asin(R[1,2])
    theta = atan2(-R[0,2]/cos(phi),R[2,2]/cos(phi))
    psi = atan2(-R[1,0]/cos(phi),R[1,1]/cos(phi))
    return phi, theta, psi 
Example 59
Project: pymoku   Author: liquidinstruments   File: _frequency_response_analyzer_data.py    MIT License 5 votes vote down vote up
def __init__(self, input_signal, gain_correction,
                 front_end_scale, output_amp):

        # Extract the length of the signal (this varies with number of
        # sweep points)
        sig_len = len(gain_correction)

        # De-interleave IQ values
        self.i_sig, self.q_sig = zip(*zip(*[iter(input_signal)] * 2))
        self.i_sig = self.i_sig[:sig_len]
        self.q_sig = self.q_sig[:sig_len]

        # Calculates magnitude of a sample given I,Q and gain correction
        # factors
        def calculate_magnitude(I, Q, G, frontend_scale):
            if I is None or Q is None:
                return None
            else:
                return 2.0 * math.sqrt(
                    (I or 0)**2 + (Q or 0)**2) * front_end_scale / (G or 1)

        self.magnitude = [calculate_magnitude(I, Q, G, front_end_scale)
                          for I, Q, G in zip(self.i_sig,
                                             self.q_sig, gain_correction)]

        # Sometimes there's a transient condition at startup where we don't
        # have a valid output_amp. Return Nones in that case in preference to
        # exploding.
        self.magnitude_dB = [None if not x else
                             20.0 * math.log10(x / output_amp)
                             if output_amp else None for x in self.magnitude]

        self.phase = [None if (I is None or Q is None)
                      else (math.atan2(Q or 0, I or 0)) / (2.0 * math.pi)
                      for I, Q in zip(self.i_sig, self.q_sig)] 
Example 60
Project: jawfish   Author: war-and-code   File: turtle.py    MIT License 5 votes vote down vote up
def towards(self, x, y=None):
        """Return the angle of the line from the turtle's position to (x, y).

        Arguments:
        x -- a number   or  a pair/vector of numbers   or   a turtle instance
        y -- a number       None                            None

        call: distance(x, y)         # two coordinates
        --or: distance((x, y))       # a pair (tuple) of coordinates
        --or: distance(vec)          # e.g. as returned by pos()
        --or: distance(mypen)        # where mypen is another turtle

        Return the angle, between the line from turtle-position to position
        specified by x, y and the turtle's start orientation. (Depends on
        modes - "standard" or "logo")

        Example (for a Turtle instance named turtle):
        >>> turtle.pos()
        (10.00, 10.00)
        >>> turtle.towards(0,0)
        225.0
        """
        if y is not None:
            pos = Vec2D(x, y)
        if isinstance(x, Vec2D):
            pos = x
        elif isinstance(x, tuple):
            pos = Vec2D(*x)
        elif isinstance(x, TNavigator):
            pos = x._position
        x, y = pos - self._position
        result = round(math.atan2(y, x)*180.0/math.pi, 10) % 360.0
        result /= self._degreesPerAU
        return (self._angleOffset + self._angleOrient*result) % self._fullcircle 
Example 61
Project: jawfish   Author: war-and-code   File: turtle.py    MIT License 5 votes vote down vote up
def heading(self):
        """ Return the turtle's current heading.

        No arguments.

        Example (for a Turtle instance named turtle):
        >>> turtle.left(67)
        >>> turtle.heading()
        67.0
        """
        x, y = self._orient
        result = round(math.atan2(y, x)*180.0/math.pi, 10) % 360.0
        result /= self._degreesPerAU
        return (self._angleOffset + self._angleOrient*result) % self._fullcircle 
Example 62
Project: jawfish   Author: war-and-code   File: turtle.py    MIT License 5 votes vote down vote up
def shapetransform(self, t11=None, t12=None, t21=None, t22=None):
        """Set or return the current transformation matrix of the turtle shape.

        Optional arguments: t11, t12, t21, t22 -- numbers.

        If none of the matrix elements are given, return the transformation
        matrix.
        Otherwise set the given elements and transform the turtleshape
        according to the matrix consisting of first row t11, t12 and
        second row t21, 22.
        Modify stretchfactor, shearfactor and tiltangle according to the
        given matrix.

        Examples (for a Turtle instance named turtle):
        >>> turtle.shape("square")
        >>> turtle.shapesize(4,2)
        >>> turtle.shearfactor(-0.5)
        >>> turtle.shapetransform()
        (4.0, -1.0, -0.0, 2.0)
        """
        #console.log("shapetransform")
        if t11 is t12 is t21 is t22 is None:
            return self._shapetrafo
        m11, m12, m21, m22 = self._shapetrafo
        if t11 is not None: m11 = t11
        if t12 is not None: m12 = t12
        if t21 is not None: m21 = t21
        if t22 is not None: m22 = t22
        if t11 * t22 - t12 * t21 == 0:
            raise TurtleGraphicsError("Bad shape transform matrix: must not be singular")
        self._shapetrafo = (m11, m12, m21, m22)
        alfa = math.atan2(-m21, m11) % (2 * math.pi)
        sa, ca = math.sin(alfa), math.cos(alfa)
        a11, a12, a21, a22 = (ca*m11 - sa*m21, ca*m12 - sa*m22,
                              sa*m11 + ca*m21, sa*m12 + ca*m22)
        self._stretchfactor = a11, a22
        self._shearfactor = a12/a22
        self._tilt = alfa
        self._update() 
Example 63
Project: AboveTustin   Author: kevinabrandon   File: geomath.py    MIT License 4 votes vote down vote up
def bearing(pointA, pointB):
	"""
	Calculates the bearing between two points.

	Found here: https://gist.github.com/jeromer/2005586

	The formulae used is the following:
	    θ = atan2(sin(Δlong).cos(lat2),
	              cos(lat1).sin(lat2) − sin(lat1).cos(lat2).cos(Δlong))

	:Parameters:
	  - `pointA: The tuple representing the latitude/longitude for the
	    first point. Latitude and longitude must be in decimal degrees
	  - `pointB: The tuple representing the latitude/longitude for the
	    second point. Latitude and longitude must be in decimal degrees

	:Returns:
	  The bearing in degrees

	:Returns Type:
	  float
	"""
	if (type(pointA) != tuple) or (type(pointB) != tuple):
		raise TypeError("Only tuples are supported as arguments")

	lat1 = math.radians(pointA[0])
	lat2 = math.radians(pointB[0])

	diffLong = math.radians(pointB[1] - pointA[1])

	x = math.sin(diffLong) * math.cos(lat2)
	y = math.cos(lat1) * math.sin(lat2) - (math.sin(lat1) 
		* math.cos(lat2) * math.cos(diffLong))

	initial_bearing = math.atan2(x, y)

	# Now we have the initial bearing but math.atan2 return values
	# from -180° to + 180° which is not what we want for a compass bearing
	# The solution is to normalize the initial bearing as shown below
	initial_bearing = math.degrees(initial_bearing)
	compass_bearing = (initial_bearing + 360) % 360

	return compass_bearing 
Example 64
Project: pyblish-win   Author: pyblish   File: test_math.py    GNU Lesser General Public License v3.0 4 votes vote down vote up
def testAtan2(self):
        self.assertRaises(TypeError, math.atan2)
        self.ftest('atan2(-1, 0)', math.atan2(-1, 0), -math.pi/2)
        self.ftest('atan2(-1, 1)', math.atan2(-1, 1), -math.pi/4)
        self.ftest('atan2(0, 1)', math.atan2(0, 1), 0)
        self.ftest('atan2(1, 1)', math.atan2(1, 1), math.pi/4)
        self.ftest('atan2(1, 0)', math.atan2(1, 0), math.pi/2)

        # math.atan2(0, x)
        self.ftest('atan2(0., -inf)', math.atan2(0., NINF), math.pi)
        self.ftest('atan2(0., -2.3)', math.atan2(0., -2.3), math.pi)
        self.ftest('atan2(0., -0.)', math.atan2(0., -0.), math.pi)
        self.assertEqual(math.atan2(0., 0.), 0.)
        self.assertEqual(math.atan2(0., 2.3), 0.)
        self.assertEqual(math.atan2(0., INF), 0.)
        self.assertTrue(math.isnan(math.atan2(0., NAN)))
        # math.atan2(-0, x)
        self.ftest('atan2(-0., -inf)', math.atan2(-0., NINF), -math.pi)
        self.ftest('atan2(-0., -2.3)', math.atan2(-0., -2.3), -math.pi)
        self.ftest('atan2(-0., -0.)', math.atan2(-0., -0.), -math.pi)
        self.assertEqual(math.atan2(-0., 0.), -0.)
        self.assertEqual(math.atan2(-0., 2.3), -0.)
        self.assertEqual(math.atan2(-0., INF), -0.)
        self.assertTrue(math.isnan(math.atan2(-0., NAN)))
        # math.atan2(INF, x)
        self.ftest('atan2(inf, -inf)', math.atan2(INF, NINF), math.pi*3/4)
        self.ftest('atan2(inf, -2.3)', math.atan2(INF, -2.3), math.pi/2)
        self.ftest('atan2(inf, -0.)', math.atan2(INF, -0.0), math.pi/2)
        self.ftest('atan2(inf, 0.)', math.atan2(INF, 0.0), math.pi/2)
        self.ftest('atan2(inf, 2.3)', math.atan2(INF, 2.3), math.pi/2)
        self.ftest('atan2(inf, inf)', math.atan2(INF, INF), math.pi/4)
        self.assertTrue(math.isnan(math.atan2(INF, NAN)))
        # math.atan2(NINF, x)
        self.ftest('atan2(-inf, -inf)', math.atan2(NINF, NINF), -math.pi*3/4)
        self.ftest('atan2(-inf, -2.3)', math.atan2(NINF, -2.3), -math.pi/2)
        self.ftest('atan2(-inf, -0.)', math.atan2(NINF, -0.0), -math.pi/2)
        self.ftest('atan2(-inf, 0.)', math.atan2(NINF, 0.0), -math.pi/2)
        self.ftest('atan2(-inf, 2.3)', math.atan2(NINF, 2.3), -math.pi/2)
        self.ftest('atan2(-inf, inf)', math.atan2(NINF, INF), -math.pi/4)
        self.assertTrue(math.isnan(math.atan2(NINF, NAN)))
        # math.atan2(+finite, x)
        self.ftest('atan2(2.3, -inf)', math.atan2(2.3, NINF), math.pi)
        self.ftest('atan2(2.3, -0.)', math.atan2(2.3, -0.), math.pi/2)
        self.ftest('atan2(2.3, 0.)', math.atan2(2.3, 0.), math.pi/2)
        self.assertEqual(math.atan2(2.3, INF), 0.)
        self.assertTrue(math.isnan(math.atan2(2.3, NAN)))
        # math.atan2(-finite, x)
        self.ftest('atan2(-2.3, -inf)', math.atan2(-2.3, NINF), -math.pi)
        self.ftest('atan2(-2.3, -0.)', math.atan2(-2.3, -0.), -math.pi/2)
        self.ftest('atan2(-2.3, 0.)', math.atan2(-2.3, 0.), -math.pi/2)
        self.assertEqual(math.atan2(-2.3, INF), -0.)
        self.assertTrue(math.isnan(math.atan2(-2.3, NAN)))
        # math.atan2(NAN, x)
        self.assertTrue(math.isnan(math.atan2(NAN, NINF)))
        self.assertTrue(math.isnan(math.atan2(NAN, -2.3)))
        self.assertTrue(math.isnan(math.atan2(NAN, -0.)))
        self.assertTrue(math.isnan(math.atan2(NAN, 0.)))
        self.assertTrue(math.isnan(math.atan2(NAN, 2.3)))
        self.assertTrue(math.isnan(math.atan2(NAN, INF)))
        self.assertTrue(math.isnan(math.atan2(NAN, NAN))) 
Example 65
Project: CozmoCommander   Author: cozmobotics   File: CozmoCommander.py    Apache License 2.0 4 votes vote down vote up
def final_adjust(charger: cozmo.objects.Charger,dist_charger=40,speed=40,critical=False):
    # Final adjustement to properly face the charger.
    # The position can be adjusted several times if 
    # the precision is critical, i.e. when climbing
    # back onto the charger.  
    global RobotGlobal
    robot = RobotGlobal
    global PI

    while(True):
        # Calculate positions
	    r_coord = [0,0,0]
	    c_coord = [0,0,0]
	    # Coordonates of robot and charger
	    r_coord[0] = robot.pose.position.x #.x .y .z, .rotation otherwise
	    r_coord[1] = robot.pose.position.y
	    r_coord[2] = robot.pose.position.z
	    r_zRot = robot.pose_angle.radians # .degrees or .radians
	    c_coord[0] = charger.pose.position.x
	    c_coord[1] = charger.pose.position.y
	    c_coord[2] = charger.pose.position.z
	    c_zRot = charger.pose.rotation.angle_z.radians

	    # Create target position 
	    # dist_charger in mm, distance if front of charger
	    c_coord[0] -=  dist_charger*math.cos(c_zRot)
	    c_coord[1] -=  dist_charger*math.sin(c_zRot)

	    # Direction and distance to target position (in front of charger)
	    distance = math.sqrt((c_coord[0]-r_coord[0])**2 + (c_coord[1]-r_coord[1])**2 + (c_coord[2]-r_coord[2])**2)
	    vect = [c_coord[0]-r_coord[0],c_coord[1]-r_coord[1],c_coord[2]-r_coord[2]]
	    # Angle of vector going from robot's origin to target's position
	    theta_t = math.atan2(vect[1],vect[0])

	    debug (2,'CHECK: Adjusting position')
	    # Face the target position
	    angle = clip_angle((theta_t-r_zRot))
	    robot.turn_in_place(radians(angle)).wait_for_completed()
	    # Drive toward the target position
	    robot.drive_straight(distance_mm(distance),speed_mmps(speed)).wait_for_completed()
	    # Face the charger
	    angle = clip_angle((c_zRot-theta_t))
	    robot.turn_in_place(radians(angle)).wait_for_completed()

        # In case the robot does not need to climb onto the charger
	    if not critical:
	        break
	    elif(check_tol(charger,dist_charger)):
	    	debug (2,'CHECK: Robot aligned relativ to the charger.')
	    	break
    return 
Example 66
Project: Perspective   Author: TypesettingTools   File: perspective.py    MIT License 4 votes vote down vote up
def unrot(coord_in, org, diag=True, get_rot=False):
    screen_z = 312.5
    shift = org.mul(-1)
    coord = [c.add(shift) for c in coord_in]
    center = intersect(coord[0::2], coord[1::2])
    center = Point(center.x, center.y, screen_z)
    rays = [Point(c.x, c.y, screen_z) for c in coord]
    f = []
    for i in range(2):
        vp1 = vec_pr(rays[0+i], center).length()
        vp2 = vec_pr(rays[2+i], center).length()
        a = rays[0+i]
        c = rays[2+i].mul(vp1/vp2)
        m = a.add(c).mul(0.5)
        r = center.z/m.z;
        a = a.mul(r)
        c = c.mul(r)
        f.append((a, c))
    (a, c), (b, d) = f[0], f[1]
    ratio = abs(dist(a, b)/dist(a, d))
    diag_diff = ((dist(a, c)-dist(b, d)))/(dist(a, c)+dist(b, d))
    n = vec_pr(vector(a, b), vector(a, c))
    n0 = vec_pr(vector(rays[0], rays[1]), vector(rays[0], rays[2]))
    flip = 1 if sc_pr(n, n0)>0 else -1
    if not get_rot:
        return diag_diff if diag else ratio#*flip
    if flip<0:
        return None
    fry = math.atan(n.x/n.z)
    s = ""
    s+= "\\fry%.2f" % (-fry/math.pi*180)
    rot_n = n.rot_y(fry)
    frx = -math.atan(rot_n.y/rot_n.z)
    if n0.z < 0:
        frx += math.pi
    s+= "\\frx%.2f" % (-frx/math.pi*180)
    n = vector(a, b)
    ab_unrot = vector(a, b).rot_y(fry).rot_x(frx)
    ac_unrot = vector(a, c).rot_y(fry).rot_x(frx)
    ad_unrot = vector(a, d).rot_y(fry).rot_x(frx)
    frz = math.atan2(ab_unrot.y, ab_unrot.x)
    s += "\\frz%.2f" % (-frz/math.pi*180)
    ad_unrot = ad_unrot.rot_z(frz)
    fax = ad_unrot.x/ad_unrot.y
    if abs(fax)>0.01:
        s += "\\fax%.2f" % (fax)
    return s 
Example 67
Project: nxt-sketcher   Author: simondolle   File: printer.py    MIT License 4 votes vote down vote up
def get_alpha_beta(x, y, structure_settings):
    r_pen = math.sqrt(math.pow(structure_settings.a, 2) + math.pow(structure_settings.s, 2))

    intersection1 = compute_circle_intersection(structure_settings.xa, 0, x, y, structure_settings.r, r_pen)
    if intersection1 is None:
        return None
    (x1, y1), (x1_prime, y1_prime) = intersection1
    alpha = math.atan2(structure_settings.xa - x1, y1) * radians_to_degrees
    alpha_prime = math.atan2(structure_settings.xa - x1_prime, y1_prime) * radians_to_degrees

    if x1 < x1_prime:
        alpha_result = alpha
        x1_actual = x1
        y1_actual = y1
    else:
        alpha_result = alpha_prime
        x1_actual = x1_prime
        y1_actual = y1_prime

    intersection_cross = compute_circle_intersection(x1_actual, y1_actual, x, y, structure_settings.a, structure_settings.s)
    if intersection_cross is None:
        return None
    (x_cross, y_cross), (x_cross_prime, y_cross_prime) = intersection_cross
    if (x_cross - x1_actual) * (y - y_cross) - (y_cross - y1_actual) * (x - x_cross) > 0:
        x_cross_actual = x_cross
        y_cross_actual = y_cross
    else:
        x_cross_actual = x_cross_prime
        y_cross_actual = y_cross_prime

    intersection2 = compute_circle_intersection(structure_settings.xb, 0, x_cross_actual, y_cross_actual, structure_settings.r, structure_settings.a)
    if intersection2 is None:
        return None
    (x2, y2), (x2_prime, y2_prime) = intersection2

    beta = math.atan2(structure_settings.xb - x2, y2) * radians_to_degrees
    beta_prime = math.atan2(structure_settings.xb - x2_prime, y2_prime) * radians_to_degrees

    if x2 > x2_prime:
        beta_result = beta
    else:
        beta_result = beta_prime

    result =  alpha_result, beta_result

    if alpha_result < -135 or 135 < alpha_result:
        return None

    if beta_result < -135 or 135 < beta_result:
        return None

    return result 
Example 68
Project: robosuite   Author: StanfordVL   File: transform_utils.py    MIT License 4 votes vote down vote up
def mat2euler(rmat, axes="sxyz"):
    """
    Converts given rotation matrix to euler angles in radian.

    Args:
        rmat: 3x3 rotation matrix
        axes: One of 24 axis sequences as string or encoded tuple

    Returns:
        converted euler angles in radian vec3 float
    """
    try:
        firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()]
    except (AttributeError, KeyError):
        firstaxis, parity, repetition, frame = axes

    i = firstaxis
    j = _NEXT_AXIS[i + parity]
    k = _NEXT_AXIS[i - parity + 1]

    M = np.array(rmat, dtype=np.float32, copy=False)[:3, :3]
    if repetition:
        sy = math.sqrt(M[i, j] * M[i, j] + M[i, k] * M[i, k])
        if sy > EPS:
            ax = math.atan2(M[i, j], M[i, k])
            ay = math.atan2(sy, M[i, i])
            az = math.atan2(M[j, i], -M[k, i])
        else:
            ax = math.atan2(-M[j, k], M[j, j])
            ay = math.atan2(sy, M[i, i])
            az = 0.0
    else:
        cy = math.sqrt(M[i, i] * M[i, i] + M[j, i] * M[j, i])
        if cy > EPS:
            ax = math.atan2(M[k, j], M[k, k])
            ay = math.atan2(-M[k, i], cy)
            az = math.atan2(M[j, i], M[i, i])
        else:
            ax = math.atan2(-M[j, k], M[j, j])
            ay = math.atan2(-M[k, i], cy)
            az = 0.0

    if parity:
        ax, ay, az = -ax, -ay, -az
    if frame:
        ax, az = az, ax
    return vec((ax, ay, az)) 
Example 69
Project: iceaddr   Author: sveinbjornt   File: add_placename_data.py    BSD 3-Clause "New" or "Revised" License 4 votes vote down vote up
def isnet93_to_wgs84(xx, yy):
    x = xx
    y = yy
    a = 6378137.0
    f = 1 / 298.257222101
    lat1 = 64.25
    lat2 = 65.75
    latc = 65.00
    lonc = 19.00
    eps = 0.00000000001

    def fx(p):
        return a * math.cos(p / rho) / math.sqrt(1 - math.pow(e * math.sin(p / rho), 2))

    def f1(p):
        return math.log((1 - p) / (1 + p))

    def f2(p):
        return f1(p) - e * f1(e * p)

    def f3(p):
        return pol1 * math.exp((f2(math.sin(p / rho)) - f2sin1) * sint / 2)

    rho = 45 / math.atan2(1.0, 1.0)
    e = math.sqrt(f * (2 - f))
    dum = f2(math.sin(lat1 / rho)) - f2(math.sin(lat2 / rho))
    sint = 2 * (math.log(fx(lat1)) - math.log(fx(lat2))) / dum
    f2sin1 = f2(math.sin(lat1 / rho))
    pol1 = fx(lat1) / sint
    polc = f3(latc) + 500000.0
    peq = (
        a
        * math.cos(latc / rho)
        / (sint * math.exp(sint * math.log((45 - latc / 2) / rho)))
    )
    pol = math.sqrt(math.pow(x - 500000, 2) + math.pow(polc - y, 2))
    lat = 90 - 2 * rho * math.atan(math.exp(math.log(pol / peq) / sint))
    lon = 0
    fact = rho * math.cos(lat / rho) / sint / pol
    fact = rho * math.cos(lat / rho) / sint / pol
    delta = 1.0
    while math.fabs(delta) > eps:
        delta = (f3(lat) - pol) * fact
        lat += delta
    lon = -(lonc + rho * math.atan((500000 - x) / (polc - y)) / sint)

    return {"lat": round(lat, 7), "lng": round(lon, 7)} 
Example 70
Project: mpu   Author: MartinThoma   File: __init__.py    MIT License 4 votes vote down vote up
def haversine_distance(origin, destination):
    """
    Calculate the Haversine distance.

    Parameters
    ----------
    origin : tuple of float
        (lat, long)
    destination : tuple of float
        (lat, long)

    Returns
    -------
    distance_in_km : float

    Examples
    --------
    >>> munich = (48.1372, 11.5756)
    >>> berlin = (52.5186, 13.4083)
    >>> round(haversine_distance(munich, berlin), 1)
    504.2

    >>> new_york_city = (40.712777777778, -74.005833333333)  # NYC
    >>> round(haversine_distance(berlin, new_york_city), 1)
    6385.3
    """
    lat1, lon1 = origin
    lat2, lon2 = destination
    if not (-90.0 <= lat1 <= 90):
        raise ValueError("lat1={:2.2f}, but must be in [-90,+90]".format(lat1))
    if not (-90.0 <= lat2 <= 90):
        raise ValueError("lat2={:2.2f}, but must be in [-90,+90]".format(lat2))
    if not (-180.0 <= lon1 <= 180):
        raise ValueError("lon1={:2.2f}, but must be in [-180,+180]"
                         .format(lat1))
    if not (-180.0 <= lon2 <= 180):
        raise ValueError("lon1={:2.2f}, but must be in [-180,+180]"
                         .format(lat1))
    radius = 6371  # km

    dlat = math_stl.radians(lat2 - lat1)
    dlon = math_stl.radians(lon2 - lon1)
    a = (math_stl.sin(dlat / 2) * math_stl.sin(dlat / 2)
         + math_stl.cos(math_stl.radians(lat1))
         * math_stl.cos(math_stl.radians(lat2))
         * math_stl.sin(dlon / 2) * math_stl.sin(dlon / 2))
    c = 2 * math_stl.atan2(math_stl.sqrt(a), math_stl.sqrt(1 - a))
    d = radius * c

    return d 
Example 71
Project: RLBotPack   Author: RLBot   File: penguin_bot.py    MIT License 4 votes vote down vote up
def Align_Car_To(self, packet, vector: Vec3, up = Vec3(0, 0, 0)):
	
	my_car = packet.game_cars[self.index]
	
	self.renderer.draw_line_3d(my_car.physics.location, (Make_Vect(my_car.physics.location) + vector.normal(200)).UI_Vec3(), self.renderer.red())
	
	car_rot = my_car.physics.rotation
	
	car_rot_vel = Make_Vect(my_car.physics.angular_velocity)
	
	local_euler = car_rot_vel.align_from(car_rot)
	
	align_local = vector.align_from(car_rot)
	
	local_up = up.align_from(car_rot)
	
	# Improving this
	rot_ang_const = 0.25
	stick_correct = 6.0
	
	a1 = math.atan2(align_local.y, align_local.x)
	a2 = math.atan2(align_local.z, align_local.x)
	
	if local_up.y == 0 and local_up.z == 0:
		a3 = 0.0
	else:
		a3 = math.atan2(local_up.y, local_up.z)
	
	yaw = correct(0.0, -a1 + local_euler.z * rot_ang_const, stick_correct)
	pitch = correct(0.0, -a2 - local_euler.y * rot_ang_const, stick_correct)
	roll = correct(0.0, -a3 - local_euler.x * rot_ang_const, stick_correct)
	
	max_input = max(abs(pitch), max(abs(roll), abs(yaw)))
	
	# yaw /= max_input
	# roll /= max_input
	# pitch /= max_input
	
	self.controller_state.yaw = constrain(yaw, -1, 1)
	self.controller_state.pitch = constrain(pitch, -1, 1)
	self.controller_state.roll = constrain(roll, -1, 1)
	
	self.controller_state.steer = constrain(yaw, -1, 1)
	
	self.renderer.draw_line_3d(my_car.physics.location, (Make_Vect(my_car.physics.location) + align_local.align_to(car_rot).normal(100)).UI_Vec3(), self.renderer.yellow()) 
Example 72
Project: RLBotPack   Author: RLBot   File: Physics.py    MIT License 4 votes vote down vote up
def Collision_R(L):
    x, y, z = L
    cx, cy, cz = wx / 2 - cR, wy / 2 - cR, wz - cR
    cx2, cz2 = wx / 2 - cR2, cR2
    cy3, cz3 = wy / 2 - cR3, cR3

    # Top Ramp X-axis
    if abs(x) > wx / 2 - cR and z > cz and (abs(x) - cx)**2 + (z - cz)**2 > (cR - R)**2:
        a = math.atan2(z - cz, abs(x) - cx) / pi * 180
        return True, [0, (90 + a) * sign(x)]

    # Top Ramp Y-axis
    if abs(y) > cy and z > cz and (abs(y) - cy)**2 + (z - cz)**2 > (cR - R)**2:
        a = math.atan2(z - cz, abs(y) - cy) / pi * 180
        return True, [(90 + a) * sign(y), 0]

    # Bottom Ramp X-axis
    elif abs(x) > cx2 and z < cz2 and (abs(x) - cx2)**2 + (z - cz2)**2 > (cR2 - R)**2:
        a = math.atan2(z - cz2, abs(x) - cx2) / pi * 180
        return True, [0, (90 + a) * sign(x)]

    # Bottom Ramp Y-axis
    elif abs(y) > cy3 and z < cz3 and abs(x) > gx / 2 - R / 2 and (abs(y) - cy3)**2 + (z - cz2)**2 > (cR3 - R)**2:
        a = math.atan2(z - cz2, abs(y) - cy3) / pi * 180
        return True, [(90 + a) * sign(y), 0]

    # Flat 45° Corner
    elif (abs(x) + abs(y) + R) / 8060 >= 1:
        return True, [90 * sign(y), 45 * sign(x)]

    # Floor
    elif z < R:
        return True, [0, 0]

    # Flat Wall X-axis
    elif abs(x) > wx / 2 - R:
        return True, [0, 90 * sign(x)]

    # Flat Wall Y-axis
    elif abs(y) > wy / 2 - R and (abs(x) > gx / 2 - R / 2 or z > gz - R / 2):
        return True, [90 * sign(y), 0]

    # Ceiling
    elif z > wz - R:
        return True, [0, 180]
        # collision bool, bounce angle (pitch, roll)
        # imagine rotating a ground plane

    else:
        return False, [0, 0] 
Example 73
Project: RLBotPack   Author: RLBot   File: catching.py    MIT License 4 votes vote down vote up
def step(self, dt):

        max_throttle_speed = 1410
        max_boost_speed = 2300

        # get the local coordinates of where the ball is, relative to the car
        # delta_local[0]: how far in front
        # delta_local[1]: how far to the left
        # delta_local[2]: how far above
        delta_local = dot(self.target_pos - self.car.pos, self.car.theta)

        # angle between car's forward direction and target position
        phi = math.atan2(delta_local[1], delta_local[0])

        if phi < -math.radians(10):
            # If the target is more than 10 degrees right from the centre, steer left
            self.controls.steer = -1
        elif phi > math.radians(10):
            # If the target is more than 10 degrees left from the centre, steer right
            self.controls.steer = 1
        else:
            # If the target is less than 10 degrees from the centre, steer straight
            self.controls.steer = phi / math.radians(10)

        if abs(phi) < math.radians(3) and not self.car.supersonic:
            self.controls.boost = True
        else:
            self.controls.boost = False

        if abs(phi) > 1.75:
            self.controls.handbrake = 1
        else:
            self.controls.handbrake = 0

        # forward velocity
        vf = dot(self.car.vel, self.car.forward())

        if vf < self.target_speed:
            self.controls.throttle = 1.0
            if self.target_speed > max_throttle_speed:
                self.controls.boost = 1
            else:
                self.controls.boost = 0
        else:
            self.controls.throttle = -1
            self.controls.boost = 0
            if norm(delta_local) < 20:
                self.controls.throttle = -norm(delta_local) / 20
            if norm(delta_local) < 10:
                self.controls.throttle = -norm(delta_local) / 10

        if self.car.supersonic:
            self.controls.boost = False

        if norm(self.car.pos - self.target_pos) < 100:
            self.finished = True 
Example 74
Project: RLBotPack   Author: RLBot   File: Derevo.py    MIT License 4 votes vote down vote up
def get_controls(self):
        if self.step == "Steer" or self.step == "Dodge2":
            self.step = "Catching"
        if self.step == "Catching":
            target = get_bounce(self)
            if target is None:
                self.step = "Defending"
            else:
                self.catching.target_pos = target[0]
                self.catching.target_speed = (distance_2d(self.info.my_car.pos, target[0]) + 50) / target[1]
                self.catching.step(self.FPS)
                self.controls = self.catching.controls
                ball = self.info.ball
                car = self.info.my_car
                if distance_2d(ball.pos, car.pos) < 150 and 65 < abs(ball.pos[2] - car.pos[2]) < 127:
                    self.step = "Dribbling"
                    self.dribble = Dribbling(self.info.my_car, self.info.ball, self.info.their_goal)
                if self.defending:
                    self.step = "Defending"
                if not self.info.my_car.on_ground:
                    self.step = "Recovery"
                ball = self.info.ball
                if abs(ball.vel[2]) < 100 and sign(self.team) * ball.vel[1] < 0 and sign(self.team) * ball.pos[1] < 0:
                    self.step = "Shooting"
        elif self.step == "Dribbling":
            self.dribble.step(self.FPS)
            self.controls = self.dribble.controls
            ball = self.info.ball
            car = self.info.my_car
            bot_to_opponent = self.info.opponents[0].pos - self.info.my_car.pos
            local_bot_to_target = dot(bot_to_opponent, self.info.my_car.theta)
            angle_front_to_target = math.atan2(local_bot_to_target[1], local_bot_to_target[0])
            opponent_is_near = norm(vec2(bot_to_opponent)) < 2000
            opponent_is_in_the_way = math.radians(-10) < angle_front_to_target < math.radians(10)
            if not (distance_2d(ball.pos, car.pos) < 150 and 65 < abs(ball.pos[2] - car.pos[2]) < 127):
                self.step = "Catching"
            if self.defending:
                self.step = "Defending"
            if opponent_is_near and opponent_is_in_the_way:
                self.step = "Dodge"
                self.dodge = AirDodge(self.info.my_car, 0.25, self.info.their_goal.center)
            if not self.info.my_car.on_ground:
                self.step = "Recovery"
        elif self.step == "Defending":
            defending(self)
        elif self.step == "Dodge":
            self.dodge.step(self.FPS)
            self.controls = self.dodge.controls
            self.controls.boost = 0
            if self.dodge.finished and self.info.my_car.on_ground:
                self.step = "Catching"
        elif self.step == "Recovery":
            self.recovery.step(self.FPS)
            self.controls = self.recovery.controls
            if self.info.my_car.on_ground:
                self.step = "Catching"
        elif self.step == "Shooting":
            shooting(self) 
Example 75
Project: RLBotPack   Author: RLBot   File: States.py    MIT License 4 votes vote down vote up
def update(self):
        self.action.step(self.agent.deltaTime)
        if self.action.finished:
            self.active = False
        return self.action.controls



# class JumpingState(baseState):
#     def __init__(self,agent, targetCode):
#         self.agent = agent
#         self.active = True
#         self.targetCode = targetCode
#         self.flip_obj = FlipStatus()
#
#     def update(self):
#         controller_state = SimpleControllerState()
#         jump = flipHandler(self.agent, self.flip_obj)
#         if jump:
#             if self.targetCode == 1:
#                 controller_state.pitch = -1
#                 controller_state.steer = 0
#                 controller_state.throttle = 1
#
#             elif self.targetCode == 0:
#                 ball_local = toLocal(self.agent.ball.location, self.agent.me)
#                 ball_angle = math.atan2(ball_local.data[1], ball_local.data[0])
#                 controller_state.jump = True
#                 controller_state.yaw = math.sin(ball_angle)
#                 pitch = -math.cos(ball_angle)
#                 controller_state.pitch = pitch
#                 if pitch > 0:
#                     controller_state.throttle = -1
#                 else:
#                     controller_state.throttle = 1
#
#             elif self.targetCode == 2:
#                 controller_state.pitch = 0
#                 controller_state.steer = 0
#                 controller_state.yaw = 0
#             elif self.targetCode == 3:
#                 controller_state.pitch = 1
#                 controller_state.steer = 0
#                 controller_state.throttle = -1
#
#             elif self.targetCode == -1:
#                 controller_state.pitch = 0
#                 controller_state.steer = 0
#                 controller_state.throttle = 0
#
#         controller_state.jump = jump
#         controller_state.boost = False
#         if self.flip_obj.flipDone:
#             self.active = False
#
#         return controller_state 
Example 76
Project: RLBotPack   Author: RLBot   File: Utilities.py    MIT License 4 votes vote down vote up
def turtleTime(agent):
    goalDistance = distance2D(agent.ball.location,Vector([0, 5100 * sign(agent.team), 200]))
    defendTarget = Vector([0, 5250 * sign(agent.team), 200])


    if agent.selectedBallPred:
        targetVec = Vector([agent.selectedBallPred.physics.location.x, agent.selectedBallPred.physics.location.y,
                            agent.selectedBallPred.physics.location.z])
        agent.ballDelay = agent.selectedBallPred.game_seconds - agent.gameInfo.seconds_elapsed

    else:
        targetVec = agent.ball.location
        agent.ballDelay = 0

    _enemyInfluenced = True
    if goalDistance < 1300:
        _enemyInfluenced = False

    flipDecider(agent,targetVec,enemyInfluenced= _enemyInfluenced)

    if distance2D(targetVec,defendTarget) < 5000:
        if ballHeadedTowardsMyGoal(agent):
            defendTarget, reposition = noOwnGoalDefense(agent,targetVec)
            if reposition:
                agent.renderCalls.append(
                    renderCall(agent.renderer.draw_line_3d, agent.me.location.data, defendTarget.data,
                               agent.renderer.blue))
                placeVecWithinArena(defendTarget)
                return testMover(agent, defendTarget, 2300)
    targ_local = toLocal(targetVec,agent.me)
    goal_local = toLocal(Vector([0, 5100 * sign(agent.team), 200]),agent.me)
    targ_angle = math.degrees(math.atan2(targ_local[1],targ_local[0]))
    goal_angle = math.degrees(math.atan2(goal_local[1],goal_local[0]))

    distance = distance2D(defendTarget,targetVec)
    oppositeVector = (getLocation(defendTarget) - targetVec).normalize()
    destination =  getLocation(defendTarget) - (oppositeVector.scale(distance - clamp(110,50,50)))
    placeVecWithinArena(destination)
    result = timeDelayedMovement(agent, destination, agent.ballDelay,True)

    destination.data[2] = 95
    agent.renderCalls.append(renderCall(agent.renderer.draw_line_3d,agent.me.location.data,destination.data,agent.renderer.blue))
    return result 
Example 77
Project: RLBotPack   Author: RLBot   File: Utilities.py    MIT License 4 votes vote down vote up
def calcTimeWithAcceleration(agent,distance,boostless = False):
    estimatedSpd = agent.currentSpd
    estimatedTime = 0
    distanceTally = 0
    if boostless:
        boostAmount = 0
    else:
        boostAmount = agent.me.boostLevel
    boostingCost = 33.3*agent.deltaTime
    linearChunk = 1600/1410
    while distanceTally < distance and estimatedTime < 6:
        if estimatedSpd < maxPossibleSpeed:
            acceleration = 1600 - (estimatedSpd*linearChunk)
            if boostAmount > 0:
                acceleration+=991
                boostAmount -= boostingCost
            if acceleration > 0:
                estimatedSpd += acceleration * agent.deltaTime
            distanceTally+= estimatedSpd*agent.deltaTime
            estimatedTime += agent.deltaTime
        else:
            #estimatedSpd += acceleration * agent.deltaTime
            distanceTally += estimatedSpd * agent.deltaTime
            estimatedTime += agent.deltaTime

    #print("friendly ended")
    return estimatedTime

# def inaccurateArrivalEstimator(agent,destination):
#     distance = clamp(math.inf,1,distance2D(agent.me.location,destination))
#     currentSpd = clamp(2300,1,agent.getCurrentSpd())
#     localTarg = toLocal(destination,agent.me)
#     if agent.forward:
#         angle = math.degrees(math.atan2(localTarg[1],localTarg[0]))
#     else:
#         angle = correctAngle(math.degrees(math.atan2(localTarg[1],localTarg[0]))-180)
#     if distance < 2000:
#         if abs(angle) > 40:
#             distance+= abs(angle)
#
#     if agent.me.boostLevel > 0:
#         maxSpd = clamp(2300,currentSpd,currentSpd+ (distance*.3))
#     else:
#         maxSpd = clamp(2200, currentSpd, currentSpd + (distance*.15))
#
#     return distance/maxSpd 
Example 78
Project: RLBotPack   Author: RLBot   File: Utilities.py    MIT License 4 votes vote down vote up
def convenientBoost(agent,targetVec):
    dist = clamp(25000,1,distance2D(agent.me.location,targetVec))
    ballDist = clamp(25000,1,distance2D(agent.me.location,agent.ball.location))
    spd = agent.getCurrentSpd()
    spd = clamp(2200,1500,spd + spd/(dist/1000))

    locTarget = toLocal(targetVec,agent.me)
    targetAngle = correctAngle(math.degrees(math.atan2(locTarget[1],locTarget[0])))


    bestBoost = None
    bestAngle = 0
    angleDisparity = 1000
    bestDist = math.inf
    goodBoosts = []
    for b in agent.boosts:
        _dist = distance2D(b.location,agent.me.location)
        if _dist < dist-500:
            localCoords = toLocal(b.location,agent.me)
            angle = correctAngle(math.degrees(math.atan2(localCoords[1],localCoords[0])))
            _angleDisparity = targetAngle - angle
            _angleDisparity = (_angleDisparity + 180) % 360 - 180
            if b.bigBoost:
                if _angleDisparity > 5:
                    _angleDisparity-=5
                elif _angleDisparity < -5:
                    _angleDisparity +=5

            if abs(_angleDisparity) < clamp(35,0,30*_dist/2000):
                goodBoosts.append(b)


    for each in goodBoosts:
        d = distance2D(each.location,agent.me.location)
        if each.bigBoost:
            d *=.7
        if d < bestDist:
            bestBoost = each
            bestDist = d


    if bestBoost != None and abs(angleDisparity) and (bestDist/spd + distance2D(bestBoost.location,targetVec)/spd < agent.ballDelay):
        if (bestDist/spd) + (distance2D(bestBoost.location,targetVec)/spd) <= agent.ballDelay or ballDist >= 3000:
            return bestBoost.location

    return None 
Example 79
Project: RLBotPack   Author: RLBot   File: marvin_atbab.py    MIT License 4 votes vote down vote up
def Collision_R(L):
    R = 93
    x,y,z = L
    wx,wy,wz = 8200, 10280, 2050    # field dimensions
    gx,gz = 1792, 640               # goal dimensions
    cR, cR2, cR3 = 520, 260, 190
    cx,cy,cz = wx/2-cR, wy/2-cR, wz-cR
    cx2,cz2 = wx/2-cR2, cR2
    cy3,cz3 = wy/2-cR3, cR3

    # Top Ramp X-axis
    if abs(x)>wx/2-cR and z>cz and (abs(x) - cx)**2 + (z - cz)**2 > (cR-R)**2:
        a = math.atan2(z-cz,abs(x)-cx)/math.pi*180
        return True, [0,(90+a)*sign(x)]

    # Top Ramp Y-axis
    if abs(y)>cy and z>cz and (abs(y) - cy)**2 + (z - cz)**2 > (cR-R)**2:
        a = math.atan2(z-cz,abs(y)-cy)/math.pi*180
        return True, [(90+a)*sign(y),0]

    # Bottom Ramp X-axis
    elif abs(x)>cx2 and z<cz2 and (abs(x) - cx2)**2 + (z - cz2)**2 > (cR2-R)**2:
        a = math.atan2(z-cz2,abs(x)-cx2)/math.pi*180
        return True, [0,(90+a)*sign(x)]

    # Bottom Ramp Y-axis
    elif abs(y)>cy3 and z<cz3 and abs(x)>gx/2-R/2 and (abs(y) - cy3)**2 + (z - cz2)**2 > (cR3-R)**2:
        a = math.atan2(z-cz2,abs(y)-cy3)/math.pi*180
        return True, [(90+a)*sign(y),0]

    # Flat 45° Corner
    elif (abs(x)+abs(y)+R)/8060 >= 1:
        return True, [90*sign(y),45*sign(x)]

    # Floor
    elif z<R:
        return True, [0,0]

    # Flat Wall X-axis
    elif abs(x)>wx/2-R:
        return True, [0,90*sign(x)]

    # Flat Wall Y-axis
    elif abs(y)>wy/2-R and (abs(x)>gx/2-R/2 or z>gz-R/2):
        return True, [90*sign(y),0]

    # Ceiling
    elif z>wz-R:
        return True, [0,180]

    else:
        return False, [0,0] 
Example 80
Project: symbolator   Author: kevinpt   File: shapes.py    MIT License 4 votes vote down vote up
def rounded_corner(start, apex, end, rad):

  # Translate all points with apex at origin
  start = (start[0] - apex[0], start[1] - apex[1])
  end = (end[0] - apex[0], end[1] - apex[1])
  
  # Get angles of each line segment
  enter_a = math.atan2(start[1], start[0]) % math.radians(360)
  leave_a = math.atan2(end[1], end[0]) % math.radians(360)
  
  #print('## enter, leave', math.degrees(enter_a), math.degrees(leave_a))
  
  # Determine bisector angle
  ea2 = abs(enter_a - leave_a)
  if ea2 > math.radians(180):
    ea2 = math.radians(360) - ea2
  bisect = ea2 / 2.0
  
  if bisect > math.radians(82): # Nearly colinear: Skip radius
    return (apex, apex, apex, -1)
  
  q = rad * math.sin(math.radians(90) - bisect) / math.sin(bisect)
  
  # Check that q is no more than half the shortest leg
  enter_leg = math.sqrt(start[0]**2 + start[1]**2)
  leave_leg = math.sqrt(end[0]**2 + end[1]**2)
  short_leg = min(enter_leg, leave_leg)
  if q > short_leg / 2:
    q = short_leg / 2
    # Compute new radius
    rad = q * math.sin(bisect) / math.sin(math.radians(90) - bisect)
    
  h = math.sqrt(q**2 + rad**2)
  
  # Center of circle

  # Determine which direction is the smallest angle to the leave point
  # Determine direction of arc
  # Rotate whole system so that enter_a is on x-axis
  delta = (leave_a - enter_a) % math.radians(360)
  if delta < math.radians(180): # CW
    bisect = enter_a + bisect
  else: # CCW
    bisect = enter_a - bisect

  #print('## Bisect2', math.degrees(bisect))
  center = (h * math.cos(bisect) + apex[0], h * math.sin(bisect) + apex[1])
  
  # Find start and end point of arcs
  start_p = (q * math.cos(enter_a) + apex[0], q * math.sin(enter_a) + apex[1])
  end_p = (q * math.cos(leave_a) + apex[0], q * math.sin(leave_a) + apex[1])
  
  return (center, start_p, end_p, rad)