def arcto(self, x, y, radius, angle1, angle2):
if self._path is None:
raise ShoebotError(_("No current path. Use beginpath() first."))
# use degrees by default
angle1 = deg2rad(angle1)
angle2 = deg2rad(angle2)
self._path.arc(x, y, radius, angle1, angle2)
def distance(point1, point2):
r"""Compute the haversine distance between two (lat,lon) points.
This method assumes the earth is a sphere with a constant radius.
The error is 0.55\%, which is good enough for most uses.
Returns distance in whole meters.
"""
(lat1, lon1) = point1
(lat2, lon2) = point2
R = 6371000 # Radius of the earth in meters
# Convert degrees to radians
phi_1 = deg2rad(lat1)
phi_2 = deg2rad(lat2)
delta_phi = deg2rad(lat2 - lat1)
delta_lambda = deg2rad(lon2 - lon1)
# Haversine
a = sin(delta_phi / 2) * sin(delta_phi / 2) + \
cos(phi_1) * cos(phi_2) * \
sin(delta_lambda / 2) * sin(delta_lambda / 2)
c = 2 * atan2(sqrt(a), sqrt(1 - a))
d = R * c
return int(round(d))
def arc(self,
x,
y,
radius,
angle1,
angle2,
type=CHORD,
draw=True,
**kwargs):
"""Draw an arc with center (x,y) between two angles in degrees.
:param x1: start x-coordinate
:param y1: start y-coordinate
:param radius: arc radius
:param angle1: start angle
:param angle2: end angle
"""
self.beginpath(**kwargs)
if type == self.PIE:
# find the coordinates of the start and end points
x1 = x + radius * cos(deg2rad(angle1))
y1 = y + radius * sin(deg2rad(angle1))
x2 = x + radius * cos(deg2rad(angle2))
y2 = y + radius * sin(deg2rad(angle2))
self.moveto(x2, y2)
self.lineto(x, y)
self.lineto(x1, y1)
# self.moveto(x, y) # uncomment to fix the "empty path" issue
self.arcto(x, y, radius, angle1, angle2)
return self.endpath(draw=draw)
def distance(point1, point2):
r"""Compute the haversine distance between two (lat,lon) points.
This method assumes the earth is a sphere with a constant radius.
The error is 0.55\%, which is good enough for most uses.
Returns distance in whole meters.
"""
(lat1, lon1) = point1
(lat2, lon2) = point2
R = 6371000 # Radius of the earth in meters
# Convert degrees to radians
phi_1 = deg2rad(lat1)
phi_2 = deg2rad(lat2)
delta_phi = deg2rad(lat2 - lat1)
delta_lambda = deg2rad(lon2 - lon1)
# Haversine
a = sin(delta_phi / 2) * sin(delta_phi / 2) + \
cos(phi_1) * cos(phi_2) * \
sin(delta_lambda / 2) * sin(delta_lambda / 2)
c = 2 * atan2(sqrt(a), sqrt(1 - a))
d = R * c
return int(round(d))
def rotate(self, degrees=0, radians=0):
### TODO change canvas to use radians
if radians:
angle = radians
else:
angle = deg2rad(degrees)
self._canvas.rotate(-angle)
def rotate(self, degrees=0, radians=0):
### TODO change canvas to use radians
if radians:
angle = radians
else:
angle = deg2rad(degrees)
self._canvas.rotate(-angle)
def updateAG(self, q, a, g, beta, dt): q0, q1, q2, q3 = q gx, gy, gz = (deg2rad(x) for x in g) ax, ay, az = a # Rate of change of quaternion from gyroscope qDot1 = 0.5 * (-q1 * gx - q2 * gy - q3 * gz) qDot2 = 0.5 * (q0 * gx + q2 * gz - q3 * gy) qDot3 = 0.5 * (q0 * gy - q1 * gz + q3 * gx) qDot4 = 0.5 * (q0 * gz + q1 * gy - q2 * gx) # Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) ax, ay, az = normalize(ax, ay, az) # Auxiliary variables to avoid repeated arithmetic _2q0 = 2.0 * q0 _2q1 = 2.0 * q1 _2q2 = 2.0 * q2 _2q3 = 2.0 * q3 _4q0 = 4.0 * q0 _4q1 = 4.0 * q1 _4q2 = 4.0 * q2 _8q1 = 8.0 * q1 _8q2 = 8.0 * q2 q0q0 = q0 * q0 q1q1 = q1 * q1 q2q2 = q2 * q2 q3q3 = q3 * q3 # Gradient decent algorithm corrective step s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay s1 = _4q1 * q3q3 - _2q3 * ax + 4.0 * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az s2 = 4.0 * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az s3 = 4.0 * q1q1 * q3 - _2q1 * ax + 4.0 * q2q2 * q3 - _2q2 * ay s0, s1, s2, s3 = normalize_q((s0, s1, s2, s3)) # Apply feedback step qDot1 -= beta * s0 qDot2 -= beta * s1 qDot3 -= beta * s2 qDot4 -= beta * s3 # Integrate rate of change of quaternion to yield quaternion q0 += qDot1 * dt q1 += qDot2 * dt q2 += qDot3 * dt q3 += qDot4 * dt q0, q1, q2, q3 = normalize_q((q0, q1, q2, q3)) self.q = (q0, q1, q2, q3) return (q0, q1, q2, q3)
def rotate(self, degrees=0, radians=0):
'''
Set the current rotation in degrees or radians.
:param degrees: Degrees to rotate
:param radians: Radians to rotate
'''
# TODO change canvas to use radians
if radians:
angle = radians
else:
angle = deg2rad(degrees)
self._canvas.rotate(-angle)
def rotate(self, degrees=0, radians=0):
'''
Set the current rotation in degrees or radians.
:param degrees: Degrees to rotate
:param radians: Radians to rotate
'''
### TODO change canvas to use radians
if radians:
angle = radians
else:
angle = deg2rad(degrees)
self._canvas.rotate(-angle)
def updateAGM(self, a, m, g, beta, dt, degrees=True):
"""
Internally, the current orientation (self.q) is tracked:
q - current quaternion Quaternion(w,x,y,z)
Args:
a - acceleration [g's], this will be normalize
m - magnetometer readings [uT], this will be normalized
g - gyro readings [rad/sec]
beta - function of sensor noise
dt - time step [sec]
degrees - if True, convert to rads/sec
Return:
q - current quaternion Quaternion(w,x,y,z)
"""
q0, q1, q2, q3 = self.q
if degrees:
gx, gy, gz = (deg2rad(x) for x in g)
else:
gx, gy, gz = g
ax, ay, az = a
mx, my, mz = m
# Rate of change of quaternion from gyroscope
qDot1 = 0.5 * (-q1 * gx - q2 * gy - q3 * gz)
qDot2 = 0.5 * (q0 * gx + q2 * gz - q3 * gy)
qDot3 = 0.5 * (q0 * gy - q1 * gz + q3 * gx)
qDot4 = 0.5 * (q0 * gz + q1 * gy - q2 * gx)
ax, ay, az = normalize(ax, ay, az)
mx, my, mz = normalize(mx, my, mz)
# Auxiliary variables to avoid repeated arithmetic
_2q0mx = 2.0 * q0 * mx
_2q0my = 2.0 * q0 * my
_2q0mz = 2.0 * q0 * mz
_2q1mx = 2.0 * q1 * mx
_2q0 = 2.0 * q0
_2q1 = 2.0 * q1
_2q2 = 2.0 * q2
_2q3 = 2.0 * q3
_2q0q2 = 2.0 * q0 * q2
_2q2q3 = 2.0 * q2 * q3
q0q0 = q0 * q0
q0q1 = q0 * q1
q0q2 = q0 * q2
q0q3 = q0 * q3
q1q1 = q1 * q1
q1q2 = q1 * q2
q1q3 = q1 * q3
q2q2 = q2 * q2
q2q3 = q2 * q3
q3q3 = q3 * q3
# Reference direction of Earth's magnetic field
hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3
hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3
_2bx = sqrt(hx * hx + hy * hy)
_2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3
_4bx = 2.0 * _2bx
_4bz = 2.0 * _2bz
# Gradient decent algorithm corrective step
s0 = -_2q2 * (2.0 * q1q3 - _2q0q2 -
ax) + _2q1 * (2.0 * q0q1 + _2q2q3 - ay) - _2bz * q2 * (
_2bx * (0.5 - q2q2 - q3q3) + _2bz *
(q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (
_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) -
my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz *
(0.5 - q1q1 - q2q2) - mz)
s1 = _2q3 * (2.0 * q1q3 - _2q0q2 - ax) + _2q0 * (
2.0 * q0q1 + _2q2q3 -
ay) - 4.0 * q1 * (1 - 2.0 * q1q1 - 2.0 * q2q2 - az) + _2bz * q3 * (
_2bx * (0.5 - q2q2 - q3q3) + _2bz *
(q1q3 - q0q2) - mx) + (_2bx * q2 + _2bz * q0) * (
_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) -
my) + (_2bx * q3 - _4bz * q1) * (_2bx *
(q0q2 + q1q3) + _2bz *
(0.5 - q1q1 - q2q2) - mz)
s2 = -_2q0 * (2.0 * q1q3 - _2q0q2 - ax) + _2q3 * (
2.0 * q0q1 + _2q2q3 -
ay) - 4.0 * q2 * (1 - 2.0 * q1q1 - 2.0 * q2q2 - az) + (
-_4bx * q2 - _2bz * q0) * (
_2bx * (0.5 - q2q2 - q3q3) + _2bz *
(q1q3 - q0q2) - mx) + (_2bx * q1 + _2bz * q3) * (
_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (
_2bx * q0 - _4bz * q2) * (_2bx *
(q0q2 + q1q3) + _2bz *
(0.5 - q1q1 - q2q2) - mz)
s3 = _2q1 * (2.0 * q1q3 - _2q0q2 - ax) + _2q2 * (
2.0 * q0q1 + _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (
_2bx * (0.5 - q2q2 - q3q3) + _2bz *
(q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (
_2bx * (q1q2 - q0q3) + _2bz *
(q0q1 + q2q3) -
my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz *
(0.5 - q1q1 - q2q2) - mz)
# s0, s1, s2, s3 = normalize_q((s0, s1, s2, s3))
s0, s1, s2, s3 = quatNorm(s0, s1, s2, s3)
# Apply feedback step
qDot1 -= beta * s0
qDot2 -= beta * s1
qDot3 -= beta * s2
qDot4 -= beta * s3
q0 += qDot1 * dt
q1 += qDot2 * dt
q2 += qDot3 * dt
q3 += qDot4 * dt
# q0, q1, q2, q3 = normalize_q((q0, q1, q2, q3))
self.q = quatNorm(q0, q1, q2, q3)
# self.q = (q0, q1, q2, q3)
return self.q
def F(n):
if n == 0: return 0
elif n == 1: return 1
else: return F(n - 1) + F(n - 2)
m = 13
# turtle.shape('turtle')
turtle.forward(F(m))
turtle.left(90)
for j in range(m, 0, -1):
a = F(j)
zlomy = 5
angle1 = 90 / (zlomy + 1)
d = 2 * a * sin(deg2rad(angle1 / 2))
turn_angle = (angle1 / 2)
for i in range(0, zlomy + 1):
turtle.left(turn_angle)
turtle.forward(d)
turtle.left(angle1 / 2)
turtle.exitonclick()
# a=200
# zlomy=20
# angle1=90/(zlomy+1)
# d=2*a*sin(deg2rad(angle1/2))
# turn_angle=(angle1/2)
#
def radial_query(self,
ra: float,
dec: float,
radius: Union[float, "Quantity"],
return_key: bool = False) -> np.recarray:
'''
Given an ra,dec coordinate and a radius (all in degrees), return the points that fall in the cone search.
The returned :class:`numpy.recarray` has the keys "ra", "dec", and (if requested), "key".
:param ra: right ascension (degrees)
:param dec: declination (degrees)
:param radius: radius (degrees), accepts a `float` value or `astropy.units.Quantity`
:param return_key: if set to True, returns the key value as provided when added to the index
:returns: if ``return_key=True``, returns a :class:`numpy.recarray` with keys ``ra,dec,key``; otherwise an array of matches, shape (n,2)
'''
if astropy_available:
if isinstance(radius, Quantity):
radius = radius.to(u.deg).value
if abs(dec) > 90:
raise ValueError(
f"The value for dec must be in the range [-90,90]; was given '{dec}'."
)
center_ra = ra
center_dec = dec
# gather matches into these lists
match_ra = list()
match_dec = list()
match_key = list()
#if isinstance(self._db, sqlite3.Connection):
fulls, partials = q3c.radial_query(self.qlsc._hprm, center_ra,
center_dec, radius)
#logger.debug(f"{center_ra=}, {center_dec=}, {radius=}")
#logger.debug(f"{fulls=}, {partials=}")
ipix_statements = list()
for min_ipix, max_ipix in fulls:
ipix_statements.append(f"(ipix>={min_ipix} AND ipix<{max_ipix})")
for min_ipix, max_ipix in partials:
ipix_statements.append(f"(ipix>={min_ipix} AND ipix<{max_ipix})")
wheres = "({0})".format(" OR ".join(ipix_statements))
query = f"SELECT ra,dec,key FROM {self.database_tablename} WHERE {wheres}"
cone_radius = pow(sin(deg2rad(radius) / 2.), 2)
connection = self._new_db_connection()
with contextlib.closing(connection.cursor()) as cursor:
try:
for ra, dec, key in cursor.execute(query):
# filter out points outside radius
if sindist(ra, dec, center_ra, center_dec) < cone_radius:
if return_key:
match_ra.append(ra)
match_dec.append(dec)
match_key.append(key)
else:
match_ra.append(ra)
match_dec.append(dec)
except sqlite3.OperationalError as e:
raise Exception(
f"Error in accessing database in radial query.\n\nQuery: \"{query}\"\n\nError: {e}"
)
if not self.memory_db_connection:
connection.close()
if return_key:
return np.core.records.fromarrays([match_ra, match_dec, match_key],
names='ra,dec,key')
else:
return np.squeeze(np.dstack((match_ra, match_dec)))
("y", c_double),
("z", c_double)]
# Reference the library convert function.
Convert = GIQLib.ConvertCoordinates
Convert.argtypes = [c_int, c_int, c_int, c_int,
POINTER(coordinates),
POINTER(coordinates), POINTER(c_int)]
Convert.restype = bool
# Setup the calling parameter values.
SRIDSource = c_int(4937) # ETRS89 Geodetic.
RevisionSource = c_int(0) # No revision required.
SRIDTarget = c_int(2157) # Irish Transverse Mercator.
RevisionTarget = c_int(2015) # Revision for ITM/GM15. This can also be 2002 for ITM/GM02.
Source = coordinates(deg2rad(-7), deg2rad(53), 100) # Longitude, Latitude, Altitude.
Target = coordinates(0, 0, 0)
Datum = c_int(13) # Malin Head datum.
CallOK = bool(False)
# Call coordinate converter.
CallOK = Convert(SRIDSource, SRIDTarget, RevisionSource, RevisionTarget, Source, Target, Datum)
# Output the result.
if CallOK:
print("Conversion result...")
print("ITM Easting: "+str(Target.x))
print("ITM Northing: "+str(Target.y))
print("Elevation: "+str(Target.z))
Datums = {
0: "None",
def __init__(self,
ast_object: starlink.Ast.Circle = None,
frame=None,
center: Iterator = None,
edge_point=None,
radius: [float, astropy.units.quantity.Quantity] = None):
'''
Parameters
----------
centerPoint : `numpy.ndarray`, list, tuple
Two elements that describe the center point of the circle in the provided frame in degrees
edgePoint : `numpy.ndarray`, list, tuple
Two elements that describe a point on the circumference of the circle in the provided frame in degrees
:param radius: float, `astropy.units.quantity.Quantity`
The radius in degrees (if float) of the circle to be created.
'''
self._uncertainty = 4.848e-6 # defaults to 1 arcsec
if ast_object:
if any(
[x is None for x in [frame, centerPoint, radius, edgePoint]]):
raise Exception(
"ASTCircle: cannot specify both 'ast_object' and any other parameter."
)
if isinstance(ast_object, starlink.Ast.Circle):
# make sure no other parameters are set
self.astObject = ast_object
return
else:
raise Exception(
"ASTCircle: The 'ast_object' provided was not of type starlink.Ast.Circle."
)
# check valid combination of parameters
# -------------------------------------
# make sure we have a frame we can work with
if frame is None:
raise Exception(
"ASTCircle: A frame must be specified when creating an ASTCircle."
)
else:
if isinstance(frame, ASTFrame):
self.frame = frame
elif isinstance(frame, starlink.Ast.Frame):
self.frame = ASTFrame(frame=frame)
else:
raise Exception(
"ASTCircle: unexpected frame type specified ('{0}').".
format(type(frame)))
if all([x is not None for x in [edge_point, radius]]):
raise ValueError(
"Both 'edge_point' and 'radius' cannot be simultaneously specified."
)
if center is None:
raise ValueError("The 'center' parameter must be set.")
if all([x is None for x in [edge_point, radius]]):
raise ValueError(
"Along with 'center', a 'radius' or 'edge_point' must be specified."
)
# input forms:
# CENTER_EDGE (0) : circle specified by center point and any point on the circumference (p1 = [float,float], p2 = [float,float])
# CENTER_RADIUS (1) : circle specified by center point and radius (p1 = [float,float], p2 = float)
input_form = None
if edge_point is None:
input_form = CENTER_RADIUS
# convert np.array types to lists so that the value can be used in 'any' and 'all' comparisons.
# if isinstance(centerPoint, np.ndarray):
# centerPoint = centerPoint.tolist()
# if isinstance(edgePoint, np.ndarray):
# edgePoint = edgePoint.tolist()
# if isinstance(centerPoint, astropy.coordinates.SkyCoord):
# centerPoint = [centerPoint.ra.to(u.deg).value, centerPoint.dec.to(u.deg).value]
# if all([centerPoint, edgePoint]) or all([centerPoint, radius]):
# if edgePoint:
# input_form = CENTER_EDGE
# else:
# input_form = CENTER_RADIUS
# else:
# raise Exception("ASTCircle: Either 'centerPoint' and 'edgePoint' OR 'centerPoint' " + \
# "and 'radius' must be specified when creating an ASTCircle.")
if isinstance(center, astropy.coordinates.SkyCoord):
p1 = [center.ra.to(u.rad).value, center.dec.to(u.rad).value]
elif isinstance(center[0], astropy.units.quantity.Quantity):
try:
p1 = [center[0].to(u.rad).value, center[1].to(u.rad).value]
except astropy.units.core.UnitConversionError as e:
# todo: be more descriptive
raise e
else:
p1 = [deg2rad(center[0]), deg2rad(center[1])]
if input_form == CENTER_EDGE:
# p1 = center point, p2 = edge point
if isinstance(edge_point, astropy.coordinates.SkyCoord):
p2 = [
edge_point.ra.to(u.rad).value,
edge_point.dec.to(u.rad).value
]
else:
p2 = [deg2rad(edge_point[0]), deg2rad(edge_point[1])]
else:
# p1 = center point, p2 = radius
if isinstance(radius, astropy.units.quantity.Quantity):
p2 = [radius.to(u.rad).value]
else:
p2 = [deg2rad(radius)]
self.astObject = Ast.Circle(self.frame.astObject,
input_form,
p1,
p2,
unc=self.uncertainty)
