# 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
def distance(origin, destination):
"""Determine distance between 2 sets of [lat,lon] in km"""

lat1, lon1 = origin
lat2, lon2 = destination

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

return d
Example 2
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
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
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
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
def _get_observation(self):
world_translation_minitaur, world_rotation_minitaur = (
self._pybullet_client.getBasePositionAndOrientation(
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
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
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
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
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))

return newSpeed
Example 11
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):
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
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.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
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
""" Return the turtle's current heading.

No arguments.

Example (for a Turtle instance named turtle):
>>> turtle.left(67)
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
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
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
"""
:param q: quaternion in w-i-j-k format
"""
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
def to_angle_axis(q0):
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
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
def y_rotation(x, y, z):
Example 21
def x_rotation(x, y, z):
Example 22
def __log__(self):
return Complex(math.log(self.abs(self)), math.atan2(self.imag, self.real))

#E^c
Example 23
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
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
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
"""

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))

return d
Example 26
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
def correction(car, ideal):
v = Make_Vect(ideal).align_from(car.physics.rotation)
return constrain_pi(math.atan2(-v.y, v.x))
Example 28
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
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
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
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
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
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
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
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))
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
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
return close_to_ball and close_to_goal and aiming_for_goal and good_angle
Example 37
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
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
def correction_to(self, ideal):

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

return correction
Example 40
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
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
def angle2(target_location,object_location):
difference = getLocation(target_location) - getLocation(object_location)
return math.atan2(difference[1], difference[0])
Example 42
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
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
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
def vec2angle(vec2):
'''
>>> vec2angle(Vec2(0, 1))
1.5707963267948966
'''
return math.atan2(vec2[1], vec2[0])
Example 46
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
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
'''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) ) )
return spherical_distance
Example 49
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
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
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
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
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
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
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
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
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
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
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
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
""" Return the turtle's current heading.

No arguments.

Example (for a Turtle instance named turtle):
>>> turtle.left(67)
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
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
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")

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
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
# 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
c_coord[0] = charger.pose.position.x
c_coord[1] = charger.pose.position.y
c_coord[2] = charger.pose.position.z

# 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])

# Face the target position
angle = clip_angle((theta_t-r_zRot))
# 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))

# 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
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)
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)
frz = math.atan2(ab_unrot.y, ab_unrot.x)
s += "\\frz%.2f" % (-frz/math.pi*180)
if abs(fax)>0.01:
s += "\\fax%.2f" % (fax)
return s
Example 67
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
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
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
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))

a = (math_stl.sin(dlat / 2) * math_stl.sin(dlat / 2)
* math_stl.sin(dlon / 2) * math_stl.sin(dlon / 2))
c = 2 * math_stl.atan2(math_stl.sqrt(a), math_stl.sqrt(1 - a))

return d
Example 71
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
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
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 the target is more than 10 degrees right from the centre, steer left
self.controls.steer = -1
# 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

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
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
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
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
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:
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
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
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
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

# 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)
bisect = ea2 / 2.0

return (apex, apex, apex, -1)

# 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

# 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])