def calc2HouseCusps(self, mundane): '''Calculates directions of Promissors to intermediate house cusps''' #aspects of proms to HCs in Zodiacal!? for i in range(len(self.chart.planets.planets)): if not self.options.promplanets[i]: continue if self.abort.abort: return pl = self.chart.planets.planets[i] rapl = pl.speculums[primdirs.PrimDirs.PLACSPECULUM][planets.Planet.RA] dsa = pl.speculums[primdirs.PrimDirs.PLACSPECULUM][planets.Planet.SA] nsa = pl.speculums[primdirs.PrimDirs.PLACSPECULUM][planets.Planet.SA] if dsa < 0.0: dsa = 180.0+dsa nsa *= -1 else: nsa = 180.0-dsa if not mundane and self.options.subzodiacal != primdirs.PrimDirs.SZPROMISSOR and self.options.subzodiacal != primdirs.PrimDirs.SZBOTH: rapl, declpl, dist = astrology.swe_cotrans(pl.data[planets.Planet.LONG], 0.0, 1.0, -self.chart.obl[0]) val = math.tan(math.radians(self.chart.place.lat))*math.tan(math.radians(declpl)) if math.fabs(val) > 1.0: continue adlat = math.degrees(math.asin(val)) dsa = 90.0+adlat nsa = 90.0-adlat self.toHCs(mundane, i, rapl, dsa, nsa, chart.Chart.CONJUNCTIO)
def __init__(self,env,x,y,size,orientation,life=200.0): object.__init__(self) self.environnement = env self.size = size self.x = x self.y = y self.lifeValue = life self.orientation = orientation self.bboxSize = int((sqrt(2)*self.size)/2) self.eyeAOW = 45.0 self.eyeRes = 0.5 # Create sensors with dummy position (center of robot) self.eyeR = eye(self.environnement,self.eyeAOW,self.orientation,self.x,self.y,self.eyeRes) self.eyeL = eye(self.environnement,self.eyeAOW,self.orientation,self.x,self.y,self.eyeRes) self.mouth = mouth(self.environnement,45.0,self.orientation,self.x,self.y) self.earR = ear(self.environnement,45.0,self.orientation,self.x,self.y) self.earL = ear(self.environnement,45.0,self.orientation,self.x,self.y) self.mySensors = [] self.mySensors.append(self.eyeR) self.mySensors.append(self.eyeL) self.mySensors.append(self.mouth) self.mySensors.append(self.earR) self.mySensors.append(self.earL) # Keep sensors relative position self.eyeAngle = 20.0 self.posEyeR = (self.eyeAngle,sqrt((self.size/2)**2+(tan((self.eyeAngle)*(pi/180.0))*(self.size/2))**2),0.0) self.posEyeL = (-self.eyeAngle,sqrt((self.size/2)**2+(tan((-self.eyeAngle)*(pi/180.0))*(self.size/2))**2),0.0) self.posMouth = (0.0,self.size/2,0) self.posEarR = (90.0,self.size/2,90.0) self.posEarL = (-90.0,self.size/2,-90.0) self.body = square(self.x,self.y,size,self.orientation,RED)
def _hintIter(self, monkey):
angle = 45
dir = 0
dA = 1
pow = self.world.P[1]
x_m = monkey[1]
y_m = monkey[0]
y = x_m*math.tan(angle*(math.pi/180)) - (self.world.g/2) * (x_m/(pow * math.cos(angle*(math.pi/180))))**2
if floor(y) < y_m:
return "Not in range"
while(floor(y) != y_m and angle >= 0 and angle <= 90):
if floor(y) < y_m:
if dir == -1:
dA = dA/2
angle += dA
theta = angle * (math.pi / 180)
y = x_m*math.tan(theta) - self.world.g * 0.5 * (x_m**2) / (pow**2) / ((math.cos(theta))**2)
dir = 1
else:
if dir == 1:
dA = dA/2
angle -= dA
theta = angle * (math.pi / 180)
y = x_m*math.tan(theta) - self.world.g * 0.5 * (x_m**2) / (pow**2) / ((math.cos(theta))**2)
dir = -1
if floor(y) == y_m:
return angle
else:
return "Angle not found"
def find_perp_error(self):
global path_list
xall = path_list[0]
zall = path_list[1]
kmin = 1000.0 #arbitrarily high value
self.delta = 1
self.eps = -tan(pi/2 - self.thetac)
self.zeta = -self.xc - self.zc*tan(pi/2 - self.thetac)
a1 = self.alpha
b1 = self.beta
c1 = self.gamma
a2 = self.delta
b2 = self.eps
c2 = self.zeta
xdm = (b2*c1 - b1*c2)/ fabs(a1*b2 - a2*b1)
zdm = (-a2*c1 + a1*c2)/ fabs(a1*b2 - a2*b1)
#Perpendicular error message
self.perppt.x = xdm
self.perppt.z = zdm
self.perppt.y = 0.0
#Perpendicular error
xde = self.lateralpt.x
zde = self.lateralpt.z
kerr = sqrt((xde-xdm)**2 + (zde-zdm)**2)
return float(kerr)
def planetstransit(date):
"""returns SHA and meridian passage for the navigational planets
"""
v = ephem.Venus()
mars = ephem.Mars()
j = ephem.Jupiter()
sat = ephem.Saturn()
obs = ephem.Observer()
obs.date = date
v.compute(date)
vsha = nadeg(2 * math.pi - ephem.degrees(v.g_ra).norm)
vtrans = time(obs.next_transit(v))
hpvenus = "%0.1f" % ((math.tan(6371 / (v.earth_distance * 149597870.7))) * 60 * 180 / math.pi)
obs.date = date
mars.compute(date)
marssha = nadeg(2 * math.pi - ephem.degrees(mars.g_ra).norm)
marstrans = time(obs.next_transit(mars))
hpmars = "%0.1f" % ((math.tan(6371 / (mars.earth_distance * 149597870.7))) * 60 * 180 / math.pi)
obs.date = date
j.compute(date)
jsha = nadeg(2 * math.pi - ephem.degrees(j.g_ra).norm)
jtrans = time(obs.next_transit(j))
obs.date = date
sat.compute(date)
satsha = nadeg(2 * math.pi - ephem.degrees(sat.g_ra).norm)
sattrans = time(obs.next_transit(sat))
return [vsha, vtrans, marssha, marstrans, jsha, jtrans, satsha, sattrans, hpmars, hpvenus]
def dayLength(date, latitude):
# Formulas from: http://www.gandraxa.com/length_of_day.xml
# I don't really understand them, and the numbers don't quite match
# what I get from calendar sites. Perhaps this is measuring the exact
# moment the center of the sun goes down, as opposed to civil twilight?
# But I think it's close enough to get the idea across.
# Tilt of earth's axis relative to its orbital plane ("obliquity of ecliptic")
axis = math.radians(23.439)
# Date of winter solstice in this year. Not quite right, but good
# enough for our purposes.
solstice = date.replace(month=12, day=21)
# If a year is a full circle, this is the angle between the solstice
# and this date, in radians. May be negative if we haven't reached the
# solstice yet.
dateAngle = (date - solstice).days * 2 * math.pi / 365.25
latitude = math.radians(latitude)
m = 1 - math.tan(latitude) * math.tan(axis * math.cos(dateAngle))
# If m is less than zero, the sun never rises; if greater than two, it never sets.
m = min(2, max(0, m))
return math.acos(1 - m) / math.pi
def s3r_ab(x1, y1, theta1, x2, y2, Db, Da, t1) :
theta_seg1 = math.atan2(y2 - y1, x2 - x1)
delta_theta1 = theta1 - theta_seg1
delta_theta1 = modulo_angle(delta_theta1)
if delta_theta1 > 0.0 :
theta1_a = theta1 - pi/2.0
else :
theta1_a = theta1 + pi/2.0
theta1_a = modulo_angle(theta1_a)
[s_bx, s_by] = signe_delta_xy(theta1)
#~ print(theta1)
print("s_bx: {0}, s_by: {1}".format(s_bx, s_by))
[s_ax, s_ay] = signe_delta_xy(theta1_a)
bx = s_bx * ((Db/t1) / sqrt(1.0 + pow(tan(theta1), 2)))
by = s_by * sqrt(pow(Db/t1, 2) - pow(bx, 2))
ax = s_ax * (((6.0*Da)/pow(t1, 2)) / sqrt(1.0 + pow(tan(theta1_a), 2)))
ay = s_ay * sqrt(pow((6.0*Da)/pow(t1, 2), 2) - pow(ax, 2))
return ([ax, ay, bx, by])
def setTreasures(self, relativeAngles):
rightAngle = 90
yDistanceFromPurpleCircle = 25
cameraDistanceFromBackgroundWall = self.robot.purpleCircle.findCenterOfMass()[0] - 20 - self.limit.getMinCorner()[0]
cameraDistanceFromLowerWall = self.limit.getMaxCorner()[1] - self.robot.purpleCircle.findCenterOfMass()[1] + 20
cameraDistanceFromUpperWall = self.robot.purpleCircle.findCenterOfMass()[1] + 105 - self.limit.getMinCorner()[1]
for cameraAngle in relativeAngles:
lowerWall = True
if cameraAngle < rightAngle:
xDistanceOfTreasureFromCamera = math.tan(math.radians(cameraAngle))*cameraDistanceFromLowerWall
treasurePosition = (cameraDistanceFromBackgroundWall - xDistanceOfTreasureFromCamera, self.limit.getMaxCorner()[1])
else:
lowerWall = False
cameraAngle = 180 - cameraAngle
xDistanceOfTreasureFromCamera = math.tan(math.radians(cameraAngle))*cameraDistanceFromUpperWall
treasurePosition = (cameraDistanceFromBackgroundWall - xDistanceOfTreasureFromCamera, self.limit.getMinCorner()[1])
treasureDistanceFromBackground = cameraDistanceFromBackgroundWall - xDistanceOfTreasureFromCamera
if treasureDistanceFromBackground < 0:
yDistanceFromCamera = (-1 * (treasureDistanceFromBackground)) / math.tan(math.radians(cameraAngle))
if lowerWall:
treasurePosition = (self.limit.getMinCorner()[0], self.limit.getMaxCorner()[1] - yDistanceFromCamera)
else:
treasurePosition = (self.limit.getMinCorner()[0], self.limit.getMinCorner()[1] + yDistanceFromCamera)
print "Tresor ajoute ", treasurePosition
self.treasures.append(treasurePosition)
def recalcCameraSphere(self):
nearPlaneDist = base.camLens.getNear()
hFov = base.camLens.getHfov()
vFov = base.camLens.getVfov()
hOff = nearPlaneDist * math.tan(deg2Rad(hFov / 2.0))
vOff = nearPlaneDist * math.tan(deg2Rad(vFov / 2.0))
camPnts = [Point3(hOff, nearPlaneDist, vOff),
Point3(-hOff, nearPlaneDist, vOff),
Point3(hOff, nearPlaneDist, -vOff),
Point3(-hOff, nearPlaneDist, -vOff),
Point3(0.0, 0.0, 0.0)]
avgPnt = Point3(0.0, 0.0, 0.0)
for camPnt in camPnts:
avgPnt = avgPnt + camPnt
avgPnt = avgPnt / len(camPnts)
sphereRadius = 0.0
for camPnt in camPnts:
dist = Vec3(camPnt - avgPnt).length()
if dist > sphereRadius:
sphereRadius = dist
avgPnt = Point3(avgPnt)
self.ccSphereNodePath.setPos(avgPnt)
self.ccSphereNodePath2.setPos(avgPnt)
self.ccSphere.setRadius(sphereRadius)
def equation_of_line_given_beacon_degree(beacon, theta): if beacon=='b1': # m=math.tan(math.radians(theta)) m=math.tan(math.radians((90 - theta) % 360 )) # m=math.tan(math.radians(270 - theta)) # c=-math.tan(math.radians(theta)) * -250 # c=250 c=m*250 elif beacon=='b2': # m=math.tan(math.radians(270 + theta)) m=math.tan(math.radians(180-theta)) c=-250 elif beacon=='b3': m=math.tan(math.radians((270 - theta))) c=m*-205 # m=math.tan(math.radians(180 + theta)) # c=-math.tan(math.radians(180 + theta)) * 250 elif beacon=='b4': m=math.tan(math.radians(180 - theta)) c=250 else: m=0 c=0 # print(m,c) return m,c
def apply_2d_transforms(context, box):
# "Transforms apply to block-level and atomic inline-level elements,
# but do not apply to elements which may be split into
# multiple inline-level boxes."
# http://www.w3.org/TR/css3-2d-transforms/#introduction
if box.style.transform and not isinstance(box, boxes.InlineBox):
border_width = box.border_width()
border_height = box.border_height()
origin_x, origin_y = box.style.transform_origin
origin_x = percentage(origin_x, border_width)
origin_y = percentage(origin_y, border_height)
origin_x += box.border_box_x()
origin_y += box.border_box_y()
context.translate(origin_x, origin_y)
for name, args in box.style.transform:
if name == 'scale':
context.scale(*args)
elif name == 'rotate':
context.rotate(args)
elif name == 'translate':
translate_x, translate_y = args
context.translate(
percentage(translate_x, border_width),
percentage(translate_y, border_height),
)
else:
if name == 'skewx':
args = (1, 0, math.tan(args), 1, 0, 0)
elif name == 'skewy':
args = (1, math.tan(args), 0, 1, 0, 0)
else:
assert name == 'matrix'
context.transform(cairo.Matrix(*args))
context.translate(-origin_x, -origin_y)
def main():
WIDTH, HEIGHT = (300, 300)
parser = GPXParser("steig.gpx")
fo = open("test.svg", "w")
gp = open("gnuplot.dat", "w")
surface = cairo.SVGSurface (fo, WIDTH, HEIGHT)
ctx = cairo.Context (surface)
ctx.scale (WIDTH/1.0, HEIGHT/1.0)
for lat, lon, ele, time in parser.tracks["Nibelungensteig on GPSies.com"]:
ctx.line_to(lon, -180/pi*log(tan(pi/4 + lat*(pi/180)/2.0)))
gp.write("%f %f %f\n"%(lat, lon, ele))
ctx.set_source_rgb(0.3, 0.2, 0.5) # Solid color
ctx.set_line_width(0.02)
ctx.stroke()
ctx.close_path()
f = open("start.txt", "r")
f.readline()
for line in f:
if line == '\r\n':
break
lat, lon = map(float, line.split('\t')[:2])
ctx.move_to(lon, -180/pi*log(tan(pi/4 + lat*(pi/180)/2.0)))
ctx.arc(lon, -180/pi*log(tan(pi/4 + lat*(pi/180)/2.0)), 0.0005, 0.0, 2.0*pi)
ctx.set_source_rgb(1.0, 0.0, 0.0)
ctx.fill_preserve()
ctx.close_path()
surface.finish()
def mapcoords(self, center_lat, center_lon, zoomlevel, width, height):
center_lon_rad = math.radians(center_lon);
center_lat_rad = math.radians(center_lat);
pos_lon_rad = math.radians(self.lon);
pos_lat_rad = math.radians(self.lat);
n = pow(2.0, zoomlevel);
centerTileX = ((center_lon + 180) / 360) * n;
centerTileY = (1 - (math.log(math.tan(center_lat_rad) + 1.0/math.cos(center_lat_rad)) / math.pi)) * n / 2.0;
#print("centerTileX = {0}".format(centerTileX))
#print("centerTileY = {0}".format(centerTileY))
posTileX = ((self.lon + 180) / 360) * n;
posTileY = (1 - (math.log(math.tan(pos_lat_rad) + 1.0/math.cos(pos_lat_rad)) / math.pi)) * n / 2.0;
#print("posTileX = {0}".format(posTileX))
#print("posTileY = {0}".format(posTileY))
diffTileX = posTileX - centerTileX
diffTileY = posTileY - centerTileY
#print("diffTileX = {0}".format(diffTileX))
#print("diffTileY = {0}".format(diffTileY))
diffPixlesX = 256 * diffTileX
diffPixlesY = 256 * diffTileY
#print("diffPixlesX = {0}".format(diffPixlesX))
#print("diffPixlesY = {0}".format(diffPixlesY))
xPos = (width / 2) + diffPixlesX
yPos = (height / 2) + diffPixlesY
return [int(xPos), int(yPos)]
def setupKinectParams(self, version=1): self.kinect_hfov = 1.0140363 # 58.1 (degrees) self.kinect_vfov = 0.813323431 # 46.6 (degrees) self.kinect_height = 240 self.kinect_width = 320 if version == 2: # Kinect v2 self.kinect_hfov = 1.23220245 # 70.6 (degrees) self.kinect_vfov = 1.04719755 # 60 (degrees) self.kinect_height = 424 self.kinect_width = 512 projectionScale = [math.tan((self.kinect_hfov * 0.5)) / (self.kinect_width * 0.5),\ math.tan((self.kinect_vfov * 0.5)) / (self.kinect_height * 0.5)] imgCenter = [float(self.kinect_width) * 0.5, float(self.kinect_height * 0.5)] xIdxImg = np.tile(np.arange(self.kinect_width), self.kinect_height).reshape((self.kinect_height, self.kinect_width)) xIdxImg = xIdxImg.astype(np.float16) yIdxImg = np.tile(np.arange(self.kinect_height), self.kinect_width).reshape((self.kinect_width, self.kinect_height)) yIdxImg = yIdxImg.T yIdxImg = yIdxImg.astype(np.float16) self.xIdxImg = ((xIdxImg - imgCenter[0]) * projectionScale[0]) self.yIdxImg = ((yIdxImg - imgCenter[1]) * projectionScale[1])
def uproj_tmerc(x, y):
easting = x - x0
northing = y
# Meridional Arc
M = northing / k0
mu = M / (a * (1.0 - e * e / 4.0 - 3.0 * e * e * e * e / 64.0 -
5.0 * e * e * e * e * e * e / 256.0))
# Footprint latitude
fp = mu + J1 * math.sin(2.0 * mu) + J2 * math.sin(4.0 * mu) + \
J3 * math.sin(6.0 * mu) + J4 * math.sin(8.0 * mu)
C1 = ep2 * math.cos(fp) * math.cos(fp)
T1 = math.tan(fp) * math.tan(fp)
# Radius of curvature in meridian plane
R1 = a * (1.0 - e * e) / \
math.pow(1.0 - e * e * math.sin(fp) * math.sin(fp), 1.5)
# Radius of curvature perpendicular to meridian plane
N1 = a / math.sqrt(1.0 - e * e * math.sin(fp) * math.sin(fp))
D = easting / (N1 * k0)
Q1 = N1 * math.tan(fp) / R1
Q2 = D * D / 2.0
Q3 = (5.0 + 3.0 * T1 + 10.0 * C1 - 4.0 * C1 * C1 - 9.0 * ep2) * \
D * D * D * D / 24.0
Q4 = (61.0 + 90.0 * T1 + 298.0 * C1 + 45.0 * T1 * T1 - 3.0 * C1 * C1 -
252.0 * ep2) * D * D * D * D * D * D / 720.0
Q5 = D
Q6 = (1.0 + 2.0 * T1 + C1) * D * D * D / 6.0
Q7 = (5.0 - 2.0 * C1 + 28.0 * T1 - 3.0 * C1 * C1 + 8.0 * ep2 +
24.0 * T1 * T1) * D * D * D * D * D / 120.0
lat = fp - Q1 * (Q2 - Q3 + Q4)
lon = math.radians(long0) + (Q5 - Q6 + Q7) / math.cos(fp)
return ( math.degrees(lat), math.degrees(lon) )
def control_head(self, hydra_msg):
#Setup the message components
headGoal = PointHeadGoal()
p = PointStamped()
p.header.frame_id = "base_link"
headGoal.pointing_frame = self.head_pointing_frame
#Get raw values from the controller
pan_axis = hydra_msg.paddles[self.left].joy[self.y_axis]
tilt_axis = hydra_msg.paddles[self.left].joy[self.x_axis]
#Check deadman's switch
if (ord(hydra_msg.paddles[self.right].buttons[self.deadman_button]) == 1):
self.head_pan = self.head_pan - pan_axis
self.head_pan = self.normalize((self.head_pan / 120.0), 1.0, 120.0)
self.head_tilt = self.head_tilt - tilt_axis
self.head_tilt = self.normalize((self.head_tilt / 30.0), 1.0, 30.0)
#Figure out actual values
p.point.x = 1.2 + (5.0 * math.tan(self.head_tilt * (math.pi / 180.0)))
p.point.y = 1.2 + (5.0 * math.tan(self.head_tilt * (math.pi / 180.0)))
p.point.z = 1.2 + (5.0 * math.tan(self.head_tilt * (math.pi / 180.0)))
#Assemble goal
headGoal.target = p
headGoal.min_duration = rospy.Duration(0.1)
headGoal.max_velocity = 1.0
#Send
self.head_client.send_goal(headGoal)
def determine_if_in_wake(xt, yt, xw, yw, k, r0, alpha): # According to Jensen Model only
# Eq. of centreline is Y = tan (d) (X - Xt) + Yt
# Distance from point to line
alpha = deg2rad(alpha + 180)
distance_to_centre = abs(- tan(alpha) * xw + yw + tan(alpha) * xt - yt) / sqrt(1.0 + tan(alpha) ** 2.0)
# print distance_to_centre
# Coordinates of the intersection between closest path from turbine in wake to centreline.
X_int = (xw + tan(alpha) * yw + tan(alpha) * (tan(alpha) * xt - yt)) / (tan(alpha) ** 2.0 + 1.0)
Y_int = (- tan(alpha) * (- xw - tan(alpha) * yw) - tan(alpha) * xt + yt) / (tan(alpha) ** 2.0 + 1.0)
# Distance from intersection point to turbine
distance_to_turbine = sqrt((X_int - xt) ** 2.0+(Y_int - yt) ** 2.0)
# Radius of wake at that distance
radius = wake_radius(r0, k, distance_to_turbine)
# print radius
if (xw - xt) * cos(alpha) + (yw - yt) * sin(alpha) <= 0.0:
if abs(radius) >= abs(distance_to_centre):
if abs(radius) >= abs(distance_to_centre) + r0:
fraction = 1.0
value = True
return fraction
elif abs(radius) < abs(distance_to_centre) + r0:
fraction = area.AreaReal(r0, radius, distance_to_centre).area()
value = True
return fraction
elif abs(radius) < abs(distance_to_centre):
if abs(radius) <= abs(distance_to_centre) - r0:
fraction = 0.0
value = False
return fraction
elif abs(radius) > abs(distance_to_centre) - r0:
fraction = area.AreaReal(r0, radius, distance_to_centre).area()
value = True
return fraction
else:
return 0.0
def predict_y_intersection(world, predict_for_x, robot, full_width=False, bounce=False):
'''
Predicts the (x, y) coordinates of the ball shot by the robot
Corrects them if it's out of the bottom_y - top_y range.
If bounce is set to True, predicts for a bounced shot
Returns None if the robot is facing the wrong direction.
'''
x = robot.x
y = robot.y
top_y = world._pitch.height - 60 if full_width else world.our_goal.y + (world.our_goal.width/2) - 30
bottom_y = 60 if full_width else world.our_goal.y - (world.our_goal.width/2) + 30
angle = robot.angle
if (robot.x < predict_for_x and not (pi/2 < angle < 3*pi/2)) or (robot.x > predict_for_x and (3*pi/2 > angle > pi/2)):
if bounce:
if not (0 <= (y + tan(angle) * (predict_for_x - x)) <= world._pitch.height):
bounce_pos = 'top' if (y + tan(angle) * (predict_for_x - x)) > world._pitch.height else 'bottom'
x += (world._pitch.height - y) / tan(angle) if bounce_pos == 'top' else (0 - y) / tan(angle)
y = world._pitch.height if bounce_pos == 'top' else 0
angle = (-angle) % (2*pi)
predicted_y = (y + tan(angle) * (predict_for_x - x))
# Correcting the y coordinate to the closest y coordinate on the goal line:
if predicted_y > top_y:
return top_y
elif predicted_y < bottom_y:
return bottom_y
return predicted_y
else:
return None
def sunset_hour_angle(latitude, sol_dec):
"""
Calculate sunset hour angle (*Ws*) from latitude and solar
declination.
Based on FAO equation 25 in Allen et al (1998).
:param latitude: Latitude [radians]. Note: *latitude* should be negative
if it in the southern hemisphere, positive if in the northern
hemisphere.
:param sol_dec: Solar declination [radians]. Can be calculated using
``sol_dec()``.
:return: Sunset hour angle [radians].
:rtype: float
"""
_check_latitude_rad(latitude)
_check_sol_dec_rad(sol_dec)
cos_sha = -math.tan(latitude) * math.tan(sol_dec)
# If tmp is >= 1 there is no sunset, i.e. 24 hours of daylight
# If tmp is <= 1 there is no sunrise, i.e. 24 hours of darkness
# See http://www.itacanet.org/the-sun-as-a-source-of-energy/
# part-3-calculating-solar-angles/
# Domain of acos is -1 <= x <= 1 radians (this is not mentioned in FAO-56!)
return math.acos(min(max(cos_sha, -1.0), 1.0))
def ApparentPlace(self):
"""Calculate annual aberration (I think :-)
Returns dRA and dDEC corrections as a tuple, in arcseconds.
# This is taken from Astronomical Formulae for Calculators, Jean Meeus,
# 3rd Ed. 1985. P:71-73.
"""
Ra = DegToRad((self.RaA / 54000.0) * 15) # Convert to degrees, and: radians
Dec = DegToRad(self.DecA / 3600.0) # Convert to degrees, and: radians
if abs((Dec / 3600) + 90) < 1e-6:
Dec = -89.999999 * 3600
if abs((Dec / 3600) - 90) < 1e-6:
Dec = 89.999999
T = (self.Time.JD - 2415020) / 36525
dPhi, dEpsi = self.Nutation(T)
L = 279.69668 + (36000.76892 * T) + (0.0003025 * T * T) # Sun's mean longitude
M = 358.47583 + (35999.04975 * T) - (0.000150 * T * T) - (0.0000033 * T * T * T)
L = DegToRad(Reduce(L)) # Reduce to 0-360, convert to radians
M = DegToRad(Reduce(M))
Epsi = 23.452294 - (0.0130125 * T) - (0.00000164 * T * T) + (0.000000503 * T * T * T)
Epsi = DegToRad(Epsi) # Convert to radians
C = (((1.919460 - (0.004789 * T) - (0.000014 * T * T)) * sin(M)) +
((0.020094 - (0.000100 * T)) * sin(2 * M)) + (0.000293 * sin(3 * M)))
Sun = L + DegToRad(C) # Sun's true longitude, in radians
dRa1 = ((cos(Epsi) + (sin(Epsi) * sin(Ra) * tan(Dec))) * dPhi) - (cos(Ra) * tan(Dec) * dEpsi)
dDec1 = (sin(Epsi) * cos(Ra) * dPhi) + (sin(Ra) * dEpsi)
dRa2 = -20.49 * (((cos(Ra) * cos(Sun) * cos(Epsi)) + (sin(Ra) * sin(Sun))) / cos(Dec))
dDec2 = -20.49 * ((cos(Sun) * cos(Epsi) * (tan(Epsi) * cos(Dec)) - (sin(Ra) * sin(Dec)))
+ (cos(Ra) * sin(Dec) * sin(Sun)))
dRA = dRa1 + dRa2 # In arcsecs
dDEC = dDec1 + dDec2 # Also in arcsecs
self.RaA += dRA
self.DecA += dDEC
def TWD97_LatLonToTWD97_TM2(cls, lat, lon):
a = cls.a
b = cls.b
lon0 = cls.lon0
k0 = cls.k0
dx = cls.dx
dy = cls.dy
e = cls.e
e2 = cls.e2
lon = (lon - floor((lon + 180) / 360) * 360) * math.pi / 180
lat = lat * math.pi / 180
V = a / (1 - e * sin(lat)**2)**0.5
T = tan(lat)**2
C = e2 * cos(lat)** 2
A = cos(lat) * (lon - lon0)
e_2 = e**2
e_3 = e**3
M = a *((1.0 - e / 4.0 - 3.0 * e_2 / 64.0 - 5.0 * e_3 / 256.0) * lat -
(3.0 * e / 8.0 + 3.0 * e_2 / 32.0 + 45.0 * e_3 / 1024.0) *
sin(2.0 * lat) + (15.0 * e_2 / 256.0 + 45.0 * e_3 / 1024.0) *
sin(4.0 * lat) - (35.0 * e_3 / 3072.0) * sin(6.0 * lat))
x = dx + k0 * V * (A + (1 - T + C) * A**3 / 6 + (5 - 18 * T + T**2 + 72 * C - 58 * e2) * A**5 / 120)
y = dy + k0 * (M + V * tan(lat) * (A**2 / 2 + (5 - T + 9 * C + 4 * C**2) * A**4/ 24 + ( 61 - 58 * T + T**2 + 600 * C - 330 * e2) * A**6 / 720))
return (x, y)
def get_trafo(self): x0 = self.origin_x.get_point_value() y0 = self.origin_y.get_point_value() sx = self.scale_x.get_value() / 100.0 sy = self.scale_y.get_value() / 100.0 shx = self.shear_x.get_value() shy = self.shear_y.get_value() if shx + shy > 85: if shx == self.transforms[3]: shy = 85 - shx else: shx = 85 - shy shx = math.pi * shx / 180.0 shy = math.pi * shy / 180.0 angle = math.pi * self.rotate.get_value() / 180.0 trafo = [sx, 0.0, 0.0, sy, x0, y0] if angle: trafo2 = [math.cos(angle), math.sin(angle), - math.sin(angle), math.cos(angle), 0.0, 0.0] trafo = libgeom.multiply_trafo(trafo, trafo2) if shx or shy: trafo2 = [1.0, math.tan(shy), math.tan(shx), 1.0, 0.0, 0.0] trafo = libgeom.multiply_trafo(trafo, trafo2) self.transforms = [sx, sy, shx, shy, angle] return trafo, [sx, sy, shx, shy, angle]
def getZD(self, md, lat, decl, umd): '''Calculates Regiomontan zenith distance ''' zd = 0.0 if md == 90.0: zd = 90.0-math.degrees(math.atan(math.sin(math.fabs(math.radians(lat))))*math.tan(math.radians(decl))) elif md < 90.0: A = math.degrees(math.atan(math.cos(math.radians(lat))*math.tan(math.radians(md)))) B = math.degrees(math.atan(math.tan(math.fabs(math.radians(lat)))*math.cos(math.radians(md)))) C = 0.0 if (decl < 0 and lat < 0) or (decl >= 0 and lat >= 0): if umd: C = B-math.fabs(decl) else: C = B+math.fabs(decl) elif (decl < 0 and lat > 0) or (decl > 0 and lat < 0): if umd: C = B+math.fabs(decl) else: C = B-math.fabs(decl) F = math.degrees(math.atan(math.sin(math.fabs(math.radians(lat)))*math.sin(math.radians(md))*math.tan(math.radians(C)))) #C and F can be negative zd = A+F return zd
def tex_triangle(tex, sommet, mesure, tex_mesure):
# Coefficient tel que hauteur = gamma * base
gamma = math.tan(math.radians(mesure[0]))*math.tan(math.radians(mesure[1]))/(math.tan(math.radians(mesure[0]))+math.tan(math.radians(mesure[1])))
# Coordonnée des sommets pour que la base ou la hauteur mesure (max-min) cm
min = -4
max = 4
if gamma < 1:
coordonnee = [min, min, max, min, round((max-min)*gamma/math.tan(math.radians(mesure[0]))+min, 4),round((max-min)*gamma+min,4)]
else:
coordonnee = [min, min, round((max-min)/gamma+min, 4), min, round((max-min)/math.tan(math.radians(mesure[0])) +min, 4) ,max]
# Angle des symboles des angles
angle = [0, mesure[0], 180 - mesure[1], 180, 180 + mesure[0], 180 + mesure[0] + mesure[2]]
## Construction
tex.append("\\begin{center}")
tex.append("\\psset{unit=0.5cm}")
tex.append("\\begin{pspicture}(%s,%s)(%s,%s)" %(coordonnee[0]-1, coordonnee[1]-1, coordonnee[2]+1, coordonnee[5]+1))
# Triangle
tex.append("\\pstTriangle(%s,%s){%s}(%s,%s){%s}(%s,%s){%s}" %(coordonnee[0], coordonnee[1], sommet[0], coordonnee[2], coordonnee[3], sommet[1], coordonnee[4], coordonnee[5], sommet[2]))
# Symbole et légende de chaque angle
for i in range(len(mesure)):
if mesure[i] == 90:
tex.append("\\pstRightAngle{%s}{%s}{%s}" % ( sommet[(i+1)%3], sommet[i], sommet[(i-1)%3]))
elif mesure[0] == mesure[1] and i < 2:
tex.append("\\pstMarkAngle{%s}{%s}{%s}{}" % ( sommet[(i+1)%3], sommet[i], sommet[(i-1)%3]))
tex.append("\\uput{0.25}[%s]{%s}(%s,%s){\\psline(0,0)(0.5,0)}" % ((angle[2*i]+angle[2*i+1])/2, (angle[2*i]+angle[2*i+1])/2, coordonnee[2*i], coordonnee[2*i+1]))
if i == 0:
tex.append("\\pstMarkAngle{%s}{%s}{%s}{$%s$}" % ( sommet[(i+1)%3], sommet[i], sommet[(i-1)%3], tex_mesure[i]))
else:
tex.append("\\pstMarkAngle{%s}{%s}{%s}{$%s$}" % ( sommet[(i+1)%3], sommet[i], sommet[(i-1)%3], tex_mesure[i]))
tex.append("\\end{pspicture}")
tex.append("\\end{center}")
def _convert_transform_to_tikz(self, transform):
"""Convert a SVG transform attribute to a list of TikZ transformations"""
#return ""
if not transform:
return []
options = []
for cmd, params in transform:
if cmd == 'translate':
x, y = params
options.append("shift={(%s,%s)}" % (round(x, 5) or '0', round(y, 5) or '0'))
# There is bug somewere.
# shift=(400,0) is not equal to xshift=400
elif cmd == 'rotate':
if params[1] or params[2]:
options.append("rotate around={%s:(%s,%s)}" % params)
else:
options.append("rotate=%s" % round(params[0], 5))
elif cmd == 'matrix':
options.append("cm={{%s,%s,%s,%s,(%s,%s)}}" % tuple(map(lambda x: round(x, 5), params)))
elif cmd == 'skewX':
options.append("xslant=%.3f" % math.tan(params[0] * math.pi / 180))
elif cmd == 'skewY':
options.append("yslant=%.3f" % math.tan(params[0] * math.pi / 180))
elif cmd == 'scale':
if params[0] == params[1]:
options.append("scale=%.3f" % params[0])
else:
options.append("xscale=%.3f,yscale=%.3f" % params)
return options
def k_eq(R, H, beta_deg):
k=0.
beta = math.radians(beta_deg)
tan_35 = math.pow(math.tan(beta), .35)
tan_23 = math.pow(math.tan(beta), .23)
k=R/H*1.15/tan_35*math.pow(H/(2.*R), .9/tan_23)
return k
def MatrixLog6(T):#Takes a T matrix SE(3) and returns a 6vector Stheta
'''
Example Input:
T = [[1,0,0,0], [0,0,-1,0], [0,1,0,3], [0,0,0,1]]
Output:
[1.5707963267948966, 0.0, 0.0, 0.0, 2.3561944901923448, 2.3561944901923457]
'''
R,p = TransToRp(T)
Rtrace = R[0][0]+R[1][1]+R[2][2]
if(R==np.eye(3)).all():
w=0
v=Normalise(p)
th=Magnitude(p)
elif(Rtrace == -1):
th = pi
w = MatrixLog3(R)
G = (1/th)*np.eye(3) - 0.5*np.asarray(VecToso3(w)) + ((1/th)-((1/(tan(th/2.0)))/2.0))*(matmult(VecToso3(w),VecToso3(w)))
v = np.dot(G,p)
else:
th = acos((Rtrace-1)/2.0)
w = so3ToVec((1/(2*np.sin(th)))*(np.subtract(R, RotInv(R))))
G = (1/th)*np.eye(3) - 0.5*np.asarray(VecToso3(w)) + ((1/th)-((1/(tan(th/2.0)))/2.0))*(matmult(VecToso3(w),VecToso3(w)))
v = np.dot(G,p)
return ([w[0]*th,w[1]*th,w[2]*th,v[0]*th,v[1]*th,v[2]*th])
def iterateRegio(self, pl, rwa, rwd, robl, rpoh, lon): okGa = okGd = True if pl.speculums[0][Planet.PMP] < 90.0 or (pl.speculums[0][Planet.PMP] >= 270.0 and pl.speculums[0][Planet.PMP] < 360.0): Ga = math.degrees(math.cos(rwa)*math.cos(robl)-math.sin(robl)*math.tan(rpoh)) if Ga != 0.0: Fa = math.degrees(math.atan(math.sin(rwa)/(math.cos(rwa)*math.cos(robl)-math.sin(robl)*math.tan(rpoh)))) if Fa >= 0.0 and Ga > 0.0: lon = Fa elif Fa < 0.0 and Ga > 0.0: lon = Fa+360.0 elif Ga < 0.0: lon = Fa+180.0 else: okGa = False else: Gd = math.degrees(math.cos(rwd)*math.cos(robl)+math.sin(robl)*math.tan(rpoh)) if Gd != 0.0: Fd = math.degrees(math.atan(math.sin(rwd)/(math.cos(rwd)*math.cos(robl)+math.sin(robl)*math.tan(rpoh)))) if Fd >= 0.0 and Gd > 0.0: lon = Fd elif Fd < 0.0 and Gd > 0.0: lon = Fd+360.0 elif Gd < 0.0: lon = Fd+180.0 else: okGd = False return okGa, okGd, lon
def ground_offset(height, pitch, roll, yaw):
'''
find the offset on the ground in meters of the center of view of the plane
given height above the ground in meters, and pitch/roll/yaw in degrees.
The yaw is from grid north. Positive yaw is clockwise
The roll is from horiznotal. Positive roll is down on the right
The pitch is from horiznotal. Positive pitch is up in the front
return result is a tuple, with meters east and north of GPS position
This is only correct for small values of pitch/roll
'''
# x/y offsets assuming the plane is pointing north
xoffset = -height * math.tan(math.radians(roll))
yoffset = height * math.tan(math.radians(pitch))
# convert to polar coordinates
distance = math.hypot(xoffset, yoffset)
angle = math.atan2(yoffset, xoffset)
# add in yaw
angle -= math.radians(yaw)
# back to rectangular coordinates
x = distance * math.cos(angle)
y = distance * math.sin(angle)
return (x, y)
def Refrac(self): # Originally in CORRECT.PAS
"""Calculate the correction for atmospheric refraction for the given coordinates.
Returns a tuple of dRA and dDEC, which are OFFSETS from the current position, in arcseconds.
"""
ObjRa = self.RaA / 54000.0 # Translate to hours from arcsec}
ObjDec = self.DecA / 3600.0 # Translate to degrees from arcsec}
z = DegToRad(90 - self.Alt) # Zenith distance in radians}
if z <= 0:
z = 1e-6
h = DegToRad((self.Time.LST - ObjRa) * 15) # Hour angle in radians}
dummy = trunc(h / (2 * pi))
h -= dummy * 2 * pi
obs = DegToRad(prefs.ObsLat) # Observatory Lat in radians}
R = -1
NewR = 0
twiggles = 0
# Calculate the value R in arc seconds}
while ((NewR - R) >= 1e-8) and (twiggles < 20):
twiggles += 1
R = NewR
Tanof = tan(z - DegToRad(R / 3600))
NewR = (R1 * Tanof) + (R2 * Tanof * Tanof * Tanof)
if twiggles > 20: # If we're very close to the horizon, the iterative solver can fail...
logger.error('correct.CalcPosition.Refrac: Too many Twiggles in refraction code!')
# Calculate dRA and dDEC in arcsec}
R = NewR
CurlR = R * 17 * (prefs.Press * 30 / 1015.92) / (460 + ((prefs.Temp * 9 / 5) + 32)) # convert Temp and Press to F and "Hg for correction}
dRA = CurlR * sin(h) * cosec(z) * cos(obs) * sec(DegToRad(ObjDec))
dDEC = CurlR * ((sin(obs) * cosec(z) * sec(DegToRad(ObjDec))) - (tan(DegToRad(ObjDec)) * cot(z)))
return dRA, dDEC
def random_affine(img, targets=(), degrees=10, translate=.1, scale=.1, shear=10, border=(0, 0)):
# torchvision.transforms.RandomAffine(degrees=(-10, 10), translate=(.1, .1), scale=(.9, 1.1), shear=(-10, 10))
# https://medium.com/uruvideo/dataset-augmentation-with-random-homographies-a8f4b44830d4
# targets = [cls, xyxy]
height = img.shape[0] + border[0] * 2 # shape(h,w,c)
width = img.shape[1] + border[1] * 2
# Rotation and Scale
R = np.eye(3)
a = random.uniform(-degrees, degrees)
# a += random.choice([-180, -90, 0, 90]) # add 90deg rotations to small rotations
s = random.uniform(1 - scale, 1 + scale)
# s = 2 ** random.uniform(-scale, scale)
R[:2] = cv2.getRotationMatrix2D(angle=a, center=(img.shape[1] / 2, img.shape[0] / 2), scale=s)
# Translation
T = np.eye(3)
T[0, 2] = random.uniform(-translate, translate) * img.shape[1] + border[1] # x translation (pixels)
T[1, 2] = random.uniform(-translate, translate) * img.shape[0] + border[0] # y translation (pixels)
# Shear
S = np.eye(3)
S[0, 1] = math.tan(random.uniform(-shear, shear) * math.pi / 180) # x shear (deg)
S[1, 0] = math.tan(random.uniform(-shear, shear) * math.pi / 180) # y shear (deg)
# Combined rotation matrix
M = S @ T @ R # ORDER IS IMPORTANT HERE!!
if (border[0] != 0) or (border[1] != 0) or (M != np.eye(3)).any(): # image changed
img = cv2.warpAffine(img, M[:2], dsize=(width, height), flags=cv2.INTER_LINEAR, borderValue=(114, 114, 114))
# Transform label coordinates
n = len(targets)
if n:
# warp points
xy = np.ones((n * 4, 3))
xy[:, :2] = targets[:, [1, 2, 3, 4, 1, 4, 3, 2]].reshape(n * 4, 2) # x1y1, x2y2, x1y2, x2y1
xy = (xy @ M.T)[:, :2].reshape(n, 8)
# create new boxes
x = xy[:, [0, 2, 4, 6]]
y = xy[:, [1, 3, 5, 7]]
xy = np.concatenate((x.min(1), y.min(1), x.max(1), y.max(1))).reshape(4, n).T
# # apply angle-based reduction of bounding boxes
# radians = a * math.pi / 180
# reduction = max(abs(math.sin(radians)), abs(math.cos(radians))) ** 0.5
# x = (xy[:, 2] + xy[:, 0]) / 2
# y = (xy[:, 3] + xy[:, 1]) / 2
# w = (xy[:, 2] - xy[:, 0]) * reduction
# h = (xy[:, 3] - xy[:, 1]) * reduction
# xy = np.concatenate((x - w / 2, y - h / 2, x + w / 2, y + h / 2)).reshape(4, n).T
# clip boxes
xy[:, [0, 2]] = xy[:, [0, 2]].clip(0, width)
xy[:, [1, 3]] = xy[:, [1, 3]].clip(0, height)
# filter candidates
i = box_candidates(box1=targets[:, 1:5].T * s, box2=xy.T)
targets = targets[i]
targets[:, 1:5] = xy[i]
return img, targets
def polysum(n,s): area=(0.25*n*s**2)/(math.tan(math.pi/n)) square_perimeter=(s*n)**2 result_sum=area+square_perimeter return round(result_sum,4)
def g(x): #return actual answer. To calc %error
return math.tan(x + math.atan(1))-x+1 #CHANGE: put F(x,y): integral of the original function f(x,y), here
from math import radians, sin, cos, tan
ang = float(input('Insira o ângulo desejado: '))
sen = sin(radians(ang))
cose = cos(radians(ang))
tang = tan(radians(ang))
print('O ângulo tem SENO de: {:.2f}\nSeno de: {:.2f}\n Cosseno de: {:.2f}'.format(sen, cose, tang))
def system(u, u1):
return [
lambda t, x: u * math.cos(x[2]),
lambda t, x: u * math.sin(x[2]),
lambda t, x: u/Lb * math.tan(u1)
]
def start(self,debug=False):
arefa = 20
center_point = (0, 0)
for i in range(10):
# 解除锁定
self.arm_state = self.arm()
self.manual_state = self.manual()
time.sleep(0.2)
r = rospy.Rate(10) # 设置数据发送频率为10hz
rcmsg = OverrideRCIn()
rcmsg.channels = [65535, 65535, 65535,
65535, 65535, 65535, 65535, 65535]
# 定义油门
# rcmsg.channels[2] = 1500
# 启动获取距离
# Thread(target=self.get_dist, args=()).start()
# 设置pid
# Thread(target=self.set_pid,args=()).start()
first_cortron = True
while self.arm_state and self.manual_state and (rospy.is_shutdown() is False):
# print("1")
#
if len(self.pointsClassification[3]) >= 0 and self.rc_in[5] > 1500:
first_cortron = True
# 清除原有图像
start_time = time.time()
cen = CenterPointEdge.CenterPointEdge(
self.pointsClassification, center_point, 1)
paths = cen.contour()
# print(paths)
# 计算单折线斜率
# dist/math.sin(theta)-x/math.tan(theta)
theta,dist,err=LineSegment.segment_Trig(paths,debug=debug)
slope = -1/math.tan(theta)
intercept = dist/math.sin(theta)
# slope, intercept, r_value, p_value, std_err = st.linregress(paths)
# 当线段拟合程度不够时使用双折线拟合
if err >0.5:
lines = LineSegment.segment2(
paths, center_point=center_point, debug=False, debug_data=paths)
# 双折线中选择k 值为负值且拟合程度大于0.75的线
# 次选拟合程度最大的
# print (lines )
if (lines[0].slope > 0 and lines[0].r_value >0.75):
slope = lines[0].slope
intercept = lines[0].intercept
elif lines[0].r_value <0.75 and lines[1].r_value == 1.0:
slope = float("inf")
intercept = 0
else:
slope = lines[1].slope
intercept = lines[1].intercept
if debug:
self.plot(paths,slope,intercept)
# 线性拟合，可以返回斜率，截距，r 值，p 值，标准误差
# slope*x - y + intercept = 0
x = []
for path in paths:
x.append(path[0])
if slope == float("inf"):
deflection = 0
# 平均x 值当做距离
left_dist = abs( list (np.average(paths,axis=0))[0])
else:
deflection = math.atan(slope)*180.0/math.pi
if deflection >0 :
deflection -=90
else:
deflection +=90
# 船到岸边的距离
left_dist = abs(
center_point[0]*slope-center_point[0]+intercept)/math.sqrt(slope**2+1)
# 均值滤波
self.left_dist = self.avg_filter(self.left_dist_queue,left_dist,self.filter_N)
# print ("defle",deflection)
# 岸线的平行的角度
left_parallel_yaw = (self.yaw - deflection) % 360
# 均值滤波
self.left_parallel_yaw = self.avg_filter(self.left_parallel_yaw_queue,left_parallel_yaw,self.filter_N)
# print ("left_parallel_yaw: ",self.left_parallel_yaw )
end_time = time.time()
# print ("time1: ", end_time-start_time)
if self.left_dist !=None and self.left_parallel_yaw != None :
# 根据与岸的距离不断调整 yaw 是其不断逼近想要的距离
self.set_dist_pid_control(
self.except_dist, self.left_dist, self.left_parallel_yaw, rcmsg)
# 调试直行程序pid
# self.set_yaw_pid_control(311,rcmsg)
# print ("rcmsg: ",rcmsg)
# self.print_log() # 打印日志
self.rc_override_pub.publish(rcmsg)
# time.sleep(0.1)
else:
# rcmsg.channels = [65535, 65535, 65535,
# 65535, 65535, 65535, 65535, 65535]
if first_cortron:
rcmsg.channels[0] = 1500
first_cortron = False
else:
rcmsg.channels[0] = 65535
# print ("rcmsg: ",rcmsg)
# print ("self.rc_in", self.rc_in)
# self.print_log() # 打印日志
self.rc_override_pub.publish(rcmsg)
time.sleep(0.1)
def img_callback(self, msg):
start_time = time.time()
# print(len(self.tiles))
font = cv2.FONT_HERSHEY_SIMPLEX
color = (0, 0, 255)
if self.frame_counter % self.skip == 0:
# self.tiles=[]
img = self.bridge.imgmsg_to_cv2(msg, "bgr8")
res = img.copy()
h, w = img.shape[:2]
M = cv2.getRotationMatrix2D((w / 2, h / 2),
(self.imu_yaw) * 180 / math.pi, 1)
img = cv2.warpAffine(img, M, (w, h))
#circle mask
circle_mask = np.zeros_like(img)
circle_mask = cv2.circle(circle_mask, (w / 2, h / 2), h / 2,
[255, 255, 255], -1)
circle_mask = circle_mask[:, :, 0]
# print(h,w)
img = self.img_correction(img)
blur = cv2.GaussianBlur(img, (7, 7), 0)
hsv = cv2.cvtColor(blur, cv2.COLOR_BGR2HSV)
mask = cv2.adaptiveThreshold(hsv[:, :, 2],255,cv2.ADAPTIVE_THRESH_GAUSSIAN_C,\
cv2.THRESH_BINARY,21, 2)
kernel = np.ones((5, 5), np.uint8)
opening = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel)
opening = 255 - opening
opening = cv2.dilate(opening, None, iterations=1)
contour_mask = 255 - opening
opening[circle_mask == 0] = 0
#fit lines to extract major direction
minLineLength = 100
lines = cv2.HoughLinesP(image=opening,rho=1,theta=np.pi/180,\
threshold=100,lines=np.array([]), minLineLength=minLineLength, maxLineGap=12)
grad = np.zeros((len(lines), 1))
i = 0
for line in lines:
#find two major gradients
x1, y1, x2, y2 = line[0][0], line[0][1], line[0][2], line[0][3]
theta = math.atan(float(y2 - y1) / (x2 - x1)) * 180 / math.pi
grad[i] = theta
i += 1
# cv2.line(img, (x1, y1), (x2, y2), (0, 0, 255), 3, cv2.LINE_AA)
cv2.line(contour_mask, (x1, y1), (x2, y2), 0, 1, cv2.LINE_AA)
hist, bin_edges = np.histogram(grad, density=False)
ind = np.argmax(hist)
best_grad = round((bin_edges[ind] + bin_edges[ind + 1]) / 2, 2)
ind = np.where(np.abs(grad - best_grad) < 10)
good_grads = grad[ind]
best_grad = np.mean(good_grads)
# contour_mask=self.mask_correction(contour_mask)
M = cv2.getRotationMatrix2D((w / 2, h / 2), best_grad, 1)
contour_mask = cv2.warpAffine(contour_mask, M, (w, h))
(_, contours, _) = cv2.findContours(contour_mask,
cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_NONE)
contour_mask = cv2.cvtColor(contour_mask, cv2.COLOR_GRAY2BGR)
areas = []
border = 0
r = []
for contour in contours:
rect = cv2.boundingRect(contour)
if rect[0] > border and rect[0] + rect[2] < w - border and rect[
1] > border and rect[3] + rect[1] < h - border:
area = int(rect[3] * rect[2])
# print(area)
ar = float(rect[2]) / rect[3]
real_ar = 0.25 / 0.12
if area > 1000 and area < 120000 and abs(ar / real_ar -
1) < 0.3:
cv2.rectangle(contour_mask, (rect[0], rect[1]),
(rect[2] + rect[0], rect[3] + rect[1]),
(0, 255, 0), 2)
areas.append(area)
r.append(rect)
areas = np.asarray(areas)
hist, bin_edges = np.histogram(areas, bins='fd', density=False)
ind = np.argmax(hist)
# best_area=(bin_edges[ind]+bin_edges[ind+1])/2
best_area = round((bin_edges[ind] + bin_edges[ind + 1]) / 2, 2)
ind = np.where(np.abs(areas - best_area) < 0.1 * best_area)
if len(ind) > 5:
good_areas = areas[ind]
best_area = np.mean(good_areas)
pred_depth = self.predict_depth(best_area)
pred_depth = pred_depth * math.cos(self.imu_pitch) * math.cos(
self.imu_roll)
for tile in self.tiles:
r = tile.one_step_update(r)
for rect in r:
self.tiles.append(Tile(rect, self.ind_count))
self.ind_count += 1
for tile in self.tiles:
if tile.alive == False:
self.tiles.remove(tile)
del_x = []
del_y = []
for tile in self.tiles:
color = self.colors[tile.ind % 20]
if len(tile.centers) > 2:
del_x.append(tile.centers[-1][1] - tile.centers[-2][1])
del_y.append(tile.centers[-1][0] - tile.centers[-2][0])
contour_mask = cv2.circle(
contour_mask,
(int(tile.centers[-1][0]), int(tile.centers[-1][1])), 5,
color, -1)
cv2.putText(contour_mask, str(tile.ind),
(tile.bb[0] + 10, tile.bb[1] + 10), font, 0.8,
color, 1, cv2.LINE_AA)
hist, bin_edges = np.histogram(np.asarray(del_x),
bins='fd',
density=False)
ind = np.argmax(hist)
best_del_x = round((bin_edges[ind] + bin_edges[ind + 1]) / 2, 2)
hist, bin_edges = np.histogram(np.asarray(del_y),
bins='fd',
density=False)
ind = np.argmax(hist)
best_del_y = round((bin_edges[ind] + bin_edges[ind + 1]) / 2, 2)
#tile real world dimension
fov_w, fov_h = 48 * math.pi / 180, 36 * math.pi / 180
px_W, px_H = 640, 480
W = 2 * pred_depth * math.tan(fov_w / 2) + 0.0001
ppm = px_W / W
self.pos_x -= best_del_x / ppm
self.pos_y -= best_del_y / ppm
self.pub_odom(self.pos_x, self.pos_y, pred_depth, best_grad)
# print(best_grad, best_area, pred_depth)
cv2.rectangle(contour_mask, (0, 0), (w, 80), (0, 0, 0), -1)
text = "direction " + str(
round(best_grad + self.imu_yaw * 180 / math.pi,
2)) + ", height: " + str(round(pred_depth, 2)) + "m"
text2 = "x: " + str(round(self.pos_x, 2)) + "m, y: " + str(
round(self.pos_y, 2)) + "m"
color = (255, 255, 255)
cv2.putText(contour_mask, text, (50, 20), font, 0.8, color, 1,
cv2.LINE_AA)
cv2.putText(contour_mask, text2, (50, 60), font, 0.8, color, 1,
cv2.LINE_AA)
opening = cv2.cvtColor(opening, cv2.COLOR_GRAY2BGR)
self.img_pub.publish(
self.bridge.cv2_to_imgmsg(np.hstack([img, contour_mask]),
"bgr8"))
self.frame_counter += 1
def operator_adv(self):
if str(
self.display.text()
) == "Math Error": # if math error is display then clear memory and display 0
self.num = 0.0
self.oldnum = 0.0
self.operatorPending = ''
self.display.setText('0')
return
clickedButton = self.sender()
clickedOperator = str(clickedButton.text())
result = float(self.display.text())
if self.isOperatorPending == True: # if operator is pending then calculate pending operation and update operator pending to true
self.num = float(self.display.text())
if self.operatorPending == u'\u002B': # addition
self.oldnum = self.num + self.oldnum
elif self.operatorPending == u'\u002D':
self.oldnum = self.oldnum - self.num
elif self.operatorPending == u'\u002A':
self.oldnum = self.num * self.oldnum
elif self.operatorPending == u'\u00F7':
if self.num == 0.0:
self.display.setText('Math Error')
return
else:
self.oldnum = self.oldnum / self.num
self.isOperatorPending == False
result = self.oldnum
if clickedOperator == u'\u00B1': # sign change
result = -result # Still have to address problem about extra 0s in float
elif clickedOperator == u'\u221A': # square root
if result < 0:
self.display.setText('Math Error')
return
else:
result = math.sqrt(result)
elif clickedOperator == u"x\N{SUPERSCRIPT TWO}": # square
result = result * result
elif clickedOperator == '1/x': # inverse
if result == 0:
self.display.setText('Math Error')
return
else:
result = 1 / result
elif clickedOperator == 'e^x': # exponential
result = math.exp(result)
elif clickedOperator == '10^x': # power(10, x)
result = pow(10, result)
elif clickedOperator == 'sin': # sin
result = math.sin(result)
elif clickedOperator == 'cos': # cos
result = math.cos(result)
elif clickedOperator == 'tan': # tan
result = math.tan(result)
elif clickedOperator == 'log': # log
if result <= 0:
self.display.setText('Math Error')
return
else:
result = math.log10(result)
self.display.setText(str(result))
return
def DrawBackbone(self, coordinates):
# box properties
numSides = 4
# start at residue position 1
cur_res = 1
# loop through residues and draw backbone
output = ""
#for (i=1;i<=count(coordinates);i++) {
for i in range(1, len(coordinates) + 1):
cur_res = i
# find next residue
next_res = cur_res - 1
if next_res < 1:
next_res = 19
# find prev residue
prev_res = cur_res + 1
if prev_res > len(coordinates):
prev_res = 2
# next-in-wheel residue orientation
wheel_next_res = cur_res - numSides
if wheel_next_res <= (
1 - numSides
): # if current res is the last res, next is the first res
wheel_next_res = len(coordinates)
elif wheel_next_res < 1:
wheel_next_res += len(coordinates) - 1
# line thickness
thickness = 6 * ((i / len(coordinates) / 1.2))
thicknessNext = 6 * (((i + 1) / len(coordinates) / 1.2))
thicknessPrev = 6 * (((i - 1) / len(coordinates) / 1.2))
# find residue base points
cur_points = self.ResiduePoints(cur_res, thickness, coordinates)
next_points = self.ResiduePoints(next_res, thicknessNext,
coordinates)
prev_points = self.ResiduePoints(prev_res, thicknessPrev,
coordinates)
# lines
cur_in = self.LineEquation(cur_points[1], cur_points[2])
cur_out = self.LineEquation(cur_points[4], cur_points[3])
next_in = self.LineEquation(next_points[1], next_points[2])
next_out = self.LineEquation(next_points[4], next_points[3])
prev_in = self.LineEquation(prev_points[1], prev_points[2])
# line points
p1 = cur_points[1]
p2 = self.LineIntercept(cur_in['m'], cur_in['b'], next_out['m'],
next_out['b'])
p3 = self.LineIntercept(cur_in['m'], cur_in['b'], next_in['m'],
next_in['b'])
p4 = self.LineIntercept(cur_in['m'], cur_in['b'], prev_in['m'],
prev_in['b'])
p5 = self.LineIntercept(cur_out['m'], cur_out['b'], prev_in['m'],
prev_in['b'])
p6 = cur_points[4]
# shorter line for the last residue
if i == len(coordinates):
move_in = self.MoveAlongLine(40, cur_in['m'], False,
cur_in['x'], cur_in['y'])
p4 = {'x': p1['x'] + move_in['x'], 'y': p1['y'] + move_in['y']}
p5 = {'x': p6['x'] + move_in['x'], 'y': p6['y'] + move_in['y']}
# draw line
points = [p2, p1, p6, p5, p4, p3]
points_txt = ""
for coord in points:
points_txt += str(coord['x']) + "," + str(coord['y']) + " "
# gradient
gradientId = uniqid()
angle = 1 / tan(self.deg2rad(cur_in['m']))
output += """
<defs>
<linearGradient id='{}' x1='100%' y1='0%' x2='0%' y2='0%' gradientTransform='rotate({})'>
<stop offset='0%' stop-color='#00cc00' stop-opacity='1'/>
<stop offset='100%' stop-color='#006600' stop-opacity='1'/>
</linearGradient>
</defs>
""".format(gradientId, angle)
lineFill = "white"
# add SVG to output
output += "<polyline stroke='black' stroke-width='0.5' points='" + points_txt + "' fill='" + lineFill + "'/>"
return output
height, width, _ = frame.shape
left, right = LaneCoord[0][0], LaneCoord[1][0]
frame = LaneCoord[2]
left_history_flag, right_history_flag = LaneCoord[3], LaneCoord[4]
if left_history_flag == True:
LaneLines = 1
right_x_min, right_y_min = right[0], right[1]
right_x_max, right_y_max = right[2], right[3]
x_offset_start = right_x_max - right_x_min
y_offset = int(height * 8 / 9)
steerAngle = SteerControl(x_offset_start, y_offset)
required_steer_angle = stabilization(old_steer_angle, steerAngle,
LaneLines)
old_steer_angle = steerAngle
steerRad = (required_steer_angle * 180 / math.pi)
val = math.tan(steerRad)
x_offset_end = int(right_x_min - height / 2 / val)
cv.line(frame, (width // 2, height), (x_offset_end, y_offset),
(0, 255, 0), 3)
elif right_history_flag == True:
LaneLines = 1
left_x_min, left_y_min = left[0], left[1]
left_x_max, left_y_max = left[2], left[3]
x_offset_start = left_x_max - left_x_min
y_offset = int(height * 8 / 9)
steerAngle = SteerControl(x_offset_start, y_offset, True)
required_steer_angle = stabilization(old_steer_angle, steerAngle,
LaneLines)
old_steer_angle = steerAngle
steerRad = (required_steer_angle * 180 / math.pi)
val = math.tan(steerRad)
def localization_callback(self, data):
"""
New message received
"""
if self.terminating == True:
self.logger.info("terminating when receive localization msg")
return
if not self.chassis_received:
self.logger.info(
"chassis not received when localization is received")
return
self.localization.CopyFrom(data)
#self.localization = data
carx = self.localization.pose.position.x
cary = self.localization.pose.position.y
carz = self.localization.pose.position.z
cartheta = self.localization.pose.heading
if math.isnan(self.chassis.speed_mps):
self.logger.warning("find nan speed_mps: %s" % str(self.chassis))
return
if math.isnan(self.chassis.steering_percentage):
self.logger.warning("find nan steering_percentage: %s" %
str(self.chassis))
return
carspeed = self.chassis.speed_mps
caracceleration = self.localization.pose.linear_acceleration_vrf.y
speed_epsilon = 1e-9
if abs(self.prev_carspeed) < speed_epsilon \
and abs(carspeed) < speed_epsilon:
caracceleration = 0.0
carsteer = self.chassis.steering_percentage
curvature = math.tan(math.radians(carsteer / 100 * 470) / 16) / 2.85
if abs(carspeed) >= speed_epsilon:
carcurvature_change_rate = (curvature -
self.carcurvature) / (carspeed * 0.01)
else:
carcurvature_change_rate = 0.0
self.carcurvature = curvature
cartime = self.localization.header.timestamp_sec
cargear = self.chassis.gear_location
if abs(carspeed) >= speed_epsilon:
if self.startmoving == False:
self.logger.info(
"carspeed !=0 and startmoving is False, Start Recording")
self.startmoving = True
if self.startmoving:
self.cars = self.cars + carspeed * 0.01
self.write(
"%s, %s, %s, %s, %s, %s, %s, %.4f, %s, %s, %s, %s, %s, %s\n" %
(carx, cary, carz, carspeed, caracceleration,
self.carcurvature, carcurvature_change_rate, cartime,
cartheta, cargear, self.cars,
self.chassis.throttle_percentage,
self.chassis.brake_percentage,
self.chassis.steering_percentage))
self.logger.debug("started moving and write data at time %s" %
cartime)
else:
self.logger.debug("not start moving, do not write data to file")
self.prev_carspeed = carspeed
# faça um programa que leia um ângulo qualquer e mostre na tela o valor do seno, cosseno e tangente desse ângulo.
import time
from math import radians, sin, cos, tan
angulo = float(input('Digite um ângulo: '))
print('...........CALCULANDO.........')
loop = 0
time.sleep(1.2)
seno = sin(radians(angulo))
cosseno = cos(radians(angulo))
tangente = tan(radians(angulo))
print('''O ângulo de {}º
tem o SENO de {:.2f}º
o COSSENO de {:.2f}º
e a TANGENTE de {:.2f}º
'''.format(angulo, seno, cosseno, tangente))
def vincentyDirect(latitude1, longitude1, alpha12, s ) :
"""
Returns the lat and long of projected point and reverse azimuth
given a reference point and a distance and azimuth to project.
lats, longs and azimuths are passed in decimal degrees
Returns ( latitude2, lambda2, alpha21 ) as a tuple
"""
f = 1.0 / 298.257223563 # WGS84
a = 6378137.0 # metres
piD4 = math.atan( 1.0 )
two_pi = piD4 * 8.0
latitude1 = latitude1 * piD4 / 45.0
longitude1 = longitude1 * piD4 / 45.0
alpha12 = alpha12 * piD4 / 45.0
if ( alpha12 < 0.0 ) :
alpha12 = alpha12 + two_pi
if ( alpha12 > two_pi ) :
alpha12 = alpha12 - two_pi
b = a * (1.0 - f)
TanU1 = (1-f) * math.tan(latitude1)
U1 = math.atan( TanU1 )
sigma1 = math.atan2( TanU1, math.cos(alpha12) )
Sinalpha = math.cos(U1) * math.sin(alpha12)
cosalpha_sq = 1.0 - Sinalpha * Sinalpha
u2 = cosalpha_sq * (a * a - b * b ) / (b * b)
A = 1.0 + (u2 / 16384) * (4096 + u2 * (-768 + u2 * \
(320 - 175 * u2) ) )
B = (u2 / 1024) * (256 + u2 * (-128 + u2 * (74 - 47 * u2) ) )
# Starting with the approximation
sigma = (s / (b * A))
last_sigma = 2.0 * sigma + 2.0 # something impossible
# Iterate the following three equations
# until there is no significant change in sigma
# two_sigma_m , delta_sigma
while ( abs( (last_sigma - sigma) / sigma) > 1.0e-9 ) :
two_sigma_m = 2 * sigma1 + sigma
delta_sigma = B * math.sin(sigma) * ( math.cos(two_sigma_m) \
+ (B/4) * (math.cos(sigma) * \
(-1 + 2 * math.pow( math.cos(two_sigma_m), 2 ) - \
(B/6) * math.cos(two_sigma_m) * \
(-3 + 4 * math.pow(math.sin(sigma), 2 )) * \
(-3 + 4 * math.pow( math.cos (two_sigma_m), 2 ))))) \
last_sigma = sigma
sigma = (s / (b * A)) + delta_sigma
latitude2 = math.atan2 ( (math.sin(U1) * math.cos(sigma) + math.cos(U1) * math.sin(sigma) * math.cos(alpha12) ), \
((1-f) * math.sqrt( math.pow(Sinalpha, 2) + \
pow(math.sin(U1) * math.sin(sigma) - math.cos(U1) * math.cos(sigma) * math.cos(alpha12), 2))))
lembda = math.atan2( (math.sin(sigma) * math.sin(alpha12 )), (math.cos(U1) * math.cos(sigma) - \
math.sin(U1) * math.sin(sigma) * math.cos(alpha12)))
C = (f/16) * cosalpha_sq * (4 + f * (4 - 3 * cosalpha_sq ))
omega = lembda - (1-C) * f * Sinalpha * \
(sigma + C * math.sin(sigma) * (math.cos(two_sigma_m) + \
C * math.cos(sigma) * (-1 + 2 * math.pow(math.cos(two_sigma_m),2) )))
longitude2 = longitude1 + omega
alpha21 = math.atan2 ( Sinalpha, (-math.sin(U1) * math.sin(sigma) + \
math.cos(U1) * math.cos(sigma) * math.cos(alpha12)))
alpha21 = alpha21 + two_pi / 2.0
if ( alpha21 < 0.0 ) :
alpha21 = alpha21 + two_pi
if ( alpha21 > two_pi ) :
alpha21 = alpha21 - two_pi
latitude2 = latitude2 * 45.0 / piD4
longitude2 = longitude2 * 45.0 / piD4
alpha21 = alpha21 * 45.0 / piD4
return latitude2, longitude2, alpha21
y = -cmx * sin(angle) + cmy * cos(angle)
rotated_mask[i] = [x, y]
rotated_mask[2] = [
40. * sin(deg2rad(90 - float(alpha) - float(phi))),
-40. * cos(deg2rad(90 - float(alpha) - float(phi)))
]
rotated_mask[3] = [
rotated_mask[2][0] - cos(deg2rad(90 - float(alpha) - float(phi))),
rotated_mask[2][1] - sin(deg2rad(90 - float(alpha) - float(phi))),
]
rotated_mask[4] = [
rotated_mask[1][0] - cos(deg2rad(90 - float(alpha) - float(phi))),
rotated_mask[1][1] - sin(deg2rad(90 - float(alpha) - float(phi))),
]
rotated_mask[5][0] = rotated_mask[0][0]
rotated_mask[5][1] = rotated_mask[4][1] - rotated_mask[5][0] * tan(
deg2rad(float(alpha) - 7))
return rotated_mask
def make_surface_plot(cases=False,
variable=False,
output='test.png',
shape=False,
zlabel='',
case_names=False,
U=False,
levels=False,
subtract_mean=False,
ticks=[],
streamlines=False,
stream_factor=1,
x4 = 2.0e-3
y1 = 2.0e-3
y2 = 1.0e-3
'''
x1 = 5.0e-3
x2 = 3.0e-3
x3 = 1.5e-3
x4 = 2.0e-3
y1 = 2.0e-3
y2 = 1.5e-3
Bangle = 5.0 * math.pi / 180.0
# the height of intersection between the magnetic and left and right boudnary of the probe
h0 = y1 - x3 * math.tan(Bangle)
h1 = y1 - (x3 + x4) * math.tan(Bangle)
D_per = 1.0
y_diffusion = math.pow(D_per * x1 / vi, 0.5)
lamada_sol = math.pow(D_per * 50.0 / vi, 0.5)
print("==========================================")
print("probe height: ", y2)
print("probe left : ", h0)
print("probe right : ", h1)
print("vi : ", vi)
print("y_diffusion : ", y_diffusion)
print("lamada_sol : ", lamada_sol)
print("==========================================")
def vinc_dist(latitude1, longitude1, latitude2, longitude2 ) :
"""
Returns the distance between two geographic points on the ellipsoid
and the forward and reverse azimuths between these points.
lats, longs and azimuths are in decimal degrees, distance in metres
Returns ( s, alpha12, alpha21 ) as a tuple
"""
f = 1.0 / 298.257223563 # WGS84
a = 6378137.0 # metres
if (abs( latitude2 - latitude1 ) < 1e-8) and ( abs( longitude2 - longitude1) < 1e-8 ) :
return 0.0, 0.0, 0.0
piD4 = math.atan( 1.0 )
two_pi = piD4 * 8.0
latitude1 = latitude1 * piD4 / 45.0
longitude1 = longitude1 * piD4 / 45.0 # unfortunately lambda is a key word!
latitude2 = latitude2 * piD4 / 45.0
longitude2 = longitude2 * piD4 / 45.0
b = a * (1.0 - f)
TanU1 = (1-f) * math.tan( latitude1 )
TanU2 = (1-f) * math.tan( latitude2 )
U1 = math.atan(TanU1)
U2 = math.atan(TanU2)
lembda = longitude2 - longitude1
last_lembda = -4000000.0 # an impossibe value
omega = lembda
# Iterate the following equations,
# until there is no significant change in lembda
while ( last_lembda < -3000000.0 or lembda != 0 and abs( (last_lembda - lembda)/lembda) > 1.0e-9 ) :
sqr_sin_sigma = pow( math.cos(U2) * math.sin(lembda), 2) + \
pow( (math.cos(U1) * math.sin(U2) - \
math.sin(U1) * math.cos(U2) * math.cos(lembda) ), 2 )
Sin_sigma = math.sqrt( sqr_sin_sigma )
Cos_sigma = math.sin(U1) * math.sin(U2) + math.cos(U1) * math.cos(U2) * math.cos(lembda)
sigma = math.atan2( Sin_sigma, Cos_sigma )
Sin_alpha = math.cos(U1) * math.cos(U2) * math.sin(lembda) / math.sin(sigma)
alpha = math.asin( Sin_alpha )
Cos2sigma_m = math.cos(sigma) - (2 * math.sin(U1) * math.sin(U2) / pow(math.cos(alpha), 2) )
C = (f/16) * pow(math.cos(alpha), 2) * (4 + f * (4 - 3 * pow(math.cos(alpha), 2)))
last_lembda = lembda
lembda = omega + (1-C) * f * math.sin(alpha) * (sigma + C * math.sin(sigma) * \
(Cos2sigma_m + C * math.cos(sigma) * (-1 + 2 * pow(Cos2sigma_m, 2) )))
u2 = pow(math.cos(alpha),2) * (a*a-b*b) / (b*b)
A = 1 + (u2/16384) * (4096 + u2 * (-768 + u2 * (320 - 175 * u2)))
B = (u2/1024) * (256 + u2 * (-128+ u2 * (74 - 47 * u2)))
delta_sigma = B * Sin_sigma * (Cos2sigma_m + (B/4) * \
(Cos_sigma * (-1 + 2 * pow(Cos2sigma_m, 2) ) - \
(B/6) * Cos2sigma_m * (-3 + 4 * sqr_sin_sigma) * \
(-3 + 4 * pow(Cos2sigma_m,2 ) )))
s = b * A * (sigma - delta_sigma)
alpha12 = math.atan2( (math.cos(U2) * math.sin(lembda)), \
(math.cos(U1) * math.sin(U2) - math.sin(U1) * math.cos(U2) * math.cos(lembda)))
alpha21 = math.atan2( (math.cos(U1) * math.sin(lembda)), \
(-math.sin(U1) * math.cos(U2) + math.cos(U1) * math.sin(U2) * math.cos(lembda)))
if ( alpha12 < 0.0 ) :
alpha12 = alpha12 + two_pi
if ( alpha12 > two_pi ) :
alpha12 = alpha12 - two_pi
alpha21 = alpha21 + two_pi / 2.0
if ( alpha21 < 0.0 ) :
alpha21 = alpha21 + two_pi
if ( alpha21 > two_pi ) :
alpha21 = alpha21 - two_pi
alpha12 = alpha12 * 45.0 / piD4
alpha21 = alpha21 * 45.0 / piD4
return s, alpha12, alpha21
TILE = 100 FPS_POS = (WIDTH - 65, 5) DIFFICULTY = 1 # minimap settings MAP_SCALE = 7 MAP_TILE = TILE // MAP_SCALE MAP_POS = (0, 0) # ray casting settings FOV = math.pi / 3 HALF_FOV = FOV / 2 NUM_RAYS = 300 MAX_DEPTH = 800 DELTA_ANGLE = FOV / NUM_RAYS DIST = NUM_RAYS / (2 * math.tan(HALF_FOV)) PROJ_COEFF = 3 * DIST * TILE SCALE = WIDTH // NUM_RAYS # texture settings (1200 x 1200) TEXTURE_WIDTH = 1200 TEXTURE_HEIGHT = 1200 TEXTURE_SCALE = TEXTURE_WIDTH // TILE # player settings player_pos = (HALF_WIDTH, HALF_HEIGHT) player_angle = 0 player_speed = 2 # colors WHITE = (255, 255, 255)
def triangulation(landmark_list, eps):
global angle_list
angle_list = getAngles(landmark_list)
(angles, landmarks) = properly_order_landmarks(landmark_list, angle_list)
alpha = (angles[1] - angles[0]) % 360.0
alpha = alpha * math.pi / 180
beta = (angles[2] - angles[1]) % 360.0
beta = beta * math.pi / 180
if alpha == 0 and beta == 0:
print("Significant measurement error (collinear).")
return
pt1 = landmarks[0]
pt2 = landmarks[1]
pt3 = landmarks[2]
v12 = unitvec(psub(pt2, pt1)) # unit vector from landmark 1 to 2
v23 = unitvec(psub(pt3, pt2)) # unit vector from landmark 2 to 3
d12 = vlen(psub(pt2, pt1)) # distance from point 1 to 2
d23 = vlen(psub(pt3, pt2)) # distance from 2 to 3
p12 = pcenter(pt1, pt2) # pt1 + 0.5*v12
p23 = pcenter(pt2, pt3)
if alpha == 0: # Robot collinear with 1 and 2
alpha = eps
if alpha == 180: # Robot collinear with 1 and 2
alpha = 180 - eps
if beta == 0: # Robot collinear with 2 and 3
beta = eps
if beta == 180: # Robot collinear with 2 and 3
beta = 180 - eps
la = 0
lb = 0
if not (alpha == 90):
# if alpha is zero, then la is zero but the tangent blows up
la = d12 / (2.0 * math.tan(alpha))
if not (beta == 90):
lb = d23 / (2.0 * math.tan(beta))
ra = d12 / (2.0 * math.sin(alpha)) # radius of circle a
rb = d23 / (2.0 * math.sin(beta)) # radius of circle b
# ca: center of circle a
ca = (p12[0] - la * v12[1], p12[1] + la * v12[0])
# cb: center of circle b
cb = (p23[0] - lb * v23[1], p23[1] + lb * v23[0])
cba = psub(ca, cb) # points from center of circle b to center of circle a
if vec_eq(ca, cb):
print("Significant measurement error (concentric).")
return
# get lengths of three segments of triangle (cb pt1 pt2) and find angle cb from the cosine rule
tri_a = seglen(cb, ca)
tri_b = seglen(cb, pt2)
tri_c = seglen(pt2, ca)
gamma = cosine_rule_get_angle(tri_a, tri_b,
tri_c) # math.asin(vlen(v12)/(2*ra))
d2r = 2 * rb * math.sin(gamma)
d2r_vec = vscale(d2r, unit_normal(cba, psub(ca, pt2)))
# d2r*(the unit normal to cba generally facing from pt2 to ca)
robot_coord = vadd(pt2, d2r_vec)
vec_robot_to_pt1 = psub(pt1, robot_coord)
heading = (v_direction(vec_robot_to_pt1) - (math.pi / 180) * angles[0])
# unit_velocity = heading_to_unit_velocity(heading)
return np.array([[robot_coord[0]], [robot_coord[1]],
[(180 / math.pi) * heading[0] % 360]])
import math
def binary_continuous(f, c, a, b):
""" Для монотонної на відрізку [a, b] функції f розв'язує рівняння
f(x) = c
:param f: Монотонна функція
:param c: Шукане значення
:param a: Ліва межа проміжку на якому здійснюється пошук
:param b: Права межа проміжку на якому здійснюється пошук
:return: Розв'язок рівняння
"""
left = a # лівий кінець відрізка
right = b # правий кінець відрізка
m = (left + right) / 2.0 # середина відрізка [left,right]
while left != m and m != right:
if f(m) < c:
left = m # [left,right] = [x,right]
else:
right = m # [left,right] = [left,x]
m = (left + right) / 2.0 # середина відрізка [left,right]
return left
if __name__ == "__main__":
print(binary_continuous(lambda x: math.tan(x) - 2.0 * x, 0, 0.5, 1.5))
def skew(self, alpha, beta):
tanAlpha = tan(alpha * pi / 180)
tanBeta = tan(beta * pi / 180)
self.transform(1, tanAlpha, tanBeta, 1, 0, 0)
def determine_if_in_wake_larsen(xt, yt, xw, yw, A, c1, ct, alpha, r0,
x0): # According to Larsen Model only
# Eq. of centreline is Y = tan (d) (X - Xt) + Yt
# Distance from point to line
alpha = deg2rad(alpha + 180)
distance_to_centre = abs(-tan(alpha) * xw + yw + tan(alpha) * xt -
yt) / sqrt(1.0 + tan(alpha)**2.0)
# print distance_to_centre
# Coordinates of the intersection between closest path from turbine in wake to centreline.
X_int = (xw + tan(alpha) * yw + tan(alpha) *
(tan(alpha) * xt - yt)) / (tan(alpha)**2.0 + 1.0)
Y_int = (-tan(alpha) * (-xw - tan(alpha) * yw) - tan(alpha) * xt +
yt) / (tan(alpha)**2.0 + 1.0)
# Distance from intersection point to turbine
distance_to_turbine = sqrt((X_int - xt)**2.0 + (Y_int - yt)**2.0)
# Radius of wake at that distance
radius = wake_radius(c1, ct, A, distance_to_turbine + x0)
# print radius
if (xw - xt) * cos(alpha) + (yw - yt) * sin(alpha) <= 0.0:
if abs(radius) >= abs(distance_to_centre):
if abs(radius) >= abs(distance_to_centre) + r0:
fraction = 1.0
value = True
return fraction, value, distance_to_centre, distance_to_turbine
elif abs(radius) < abs(distance_to_centre) + r0:
fraction = area.AreaReal(r0, radius, distance_to_centre).area()
value = True
return fraction, value, distance_to_centre, distance_to_turbine
elif abs(radius) < abs(distance_to_centre):
if abs(radius) <= abs(distance_to_centre) - r0:
fraction = 0.0
value = False
return fraction, value, distance_to_centre, distance_to_turbine
elif abs(radius) > abs(distance_to_centre) - r0:
fraction = area.AreaReal(r0, radius, distance_to_centre).area()
value = True
return fraction, value, distance_to_centre, distance_to_turbine
else:
return 0.0, False, distance_to_centre, distance_to_turbine
def random_affine(img, targets=None, degrees=(-10, 10), translate=(.1, .1), scale=(.9, 1.1), shear=(-2, 2),
borderValue=(127.5, 127.5, 127.5)):
# torchvision.transforms.RandomAffine(degrees=(-10, 10), translate=(.1, .1), scale=(.9, 1.1), shear=(-10, 10))
# https://medium.com/uruvideo/dataset-augmentation-with-random-homographies-a8f4b44830d4
border = 0 # width of added border (optional)
height = img.shape[0]
width = img.shape[1]
# Rotation and Scale
R = np.eye(3)
a = random.random() * (degrees[1] - degrees[0]) + degrees[0]
# a += random.choice([-180, -90, 0, 90]) # 90deg rotations added to small rotations
s = random.random() * (scale[1] - scale[0]) + scale[0]
R[:2] = cv2.getRotationMatrix2D(angle=a, center=(img.shape[1] / 2, img.shape[0] / 2), scale=s)
# Translation
T = np.eye(3)
T[0, 2] = (random.random() * 2 - 1) * translate[0] * img.shape[0] + border # x translation (pixels)
T[1, 2] = (random.random() * 2 - 1) * translate[1] * img.shape[1] + border # y translation (pixels)
# Shear
S = np.eye(3)
S[0, 1] = math.tan((random.random() * (shear[1] - shear[0]) + shear[0]) * math.pi / 180) # x shear (deg)
S[1, 0] = math.tan((random.random() * (shear[1] - shear[0]) + shear[0]) * math.pi / 180) # y shear (deg)
M = S @ T @ R # Combined rotation matrix. ORDER IS IMPORTANT HERE!!
imw = cv2.warpPerspective(img, M, dsize=(width, height), flags=cv2.INTER_LINEAR,
borderValue=borderValue) # BGR order borderValue
# Return warped points also
if targets is not None:
if len(targets) > 0:
n = targets.shape[0]
points = targets[:, 2:6].copy()
area0 = (points[:, 2] - points[:, 0]) * (points[:, 3] - points[:, 1])
# warp points
xy = np.ones((n * 4, 3))
xy[:, :2] = points[:, [0, 1, 2, 3, 0, 3, 2, 1]].reshape(n * 4, 2) # x1y1, x2y2, x1y2, x2y1
xy = (xy @ M.T)[:, :2].reshape(n, 8)
# create new boxes
x = xy[:, [0, 2, 4, 6]]
y = xy[:, [1, 3, 5, 7]]
xy = np.concatenate((x.min(1), y.min(1), x.max(1), y.max(1))).reshape(4, n).T
# apply angle-based reduction
radians = a * math.pi / 180
reduction = max(abs(math.sin(radians)), abs(math.cos(radians))) ** 0.5
x = (xy[:, 2] + xy[:, 0]) / 2
y = (xy[:, 3] + xy[:, 1]) / 2
w = (xy[:, 2] - xy[:, 0]) * reduction
h = (xy[:, 3] - xy[:, 1]) * reduction
xy = np.concatenate((x - w / 2, y - h / 2, x + w / 2, y + h / 2)).reshape(4, n).T
# reject warped points outside of image
np.clip(xy[:, 0], 0, width, out=xy[:, 0])
np.clip(xy[:, 2], 0, width, out=xy[:, 2])
np.clip(xy[:, 1], 0, height, out=xy[:, 1])
np.clip(xy[:, 3], 0, height, out=xy[:, 3])
w = xy[:, 2] - xy[:, 0]
h = xy[:, 3] - xy[:, 1]
area = w * h
ar = np.maximum(w / (h + 1e-16), h / (w + 1e-16))
i = (w > 4) & (h > 4) & (area / (area0 + 1e-16) > 0.1) & (ar < 10)
targets = targets[i]
targets[:, 2:6] = xy[i]
return imw, targets, M
else:
return imw
def __ckeck_ok(f_atv, f_cine_data):
"""
verifica condições da aeronave para o direcionamento ao fixo
@param f_atv: ponteiro para aeronave
@param f_cine_data: ponteiro para pilha
"""
# check input
assert f_atv
assert f_cine_data
# active flight ?
if (not f_atv.v_atv_ok) or (ldefs.E_ATIVA != f_atv.en_trf_est_atv):
# logger
l_log = logging.getLogger("prc_dir_fixo::__check_ok")
l_log.setLevel(logging.ERROR)
l_log.error("<E01: aeronave não ativa.")
# cai fora...
return
# aponta para o fixo a ser interceptado e valida ponteiro
l_fix = f_atv.ptr_atv_fix_prc
# M_LOG.debug("prc_dir_fixo::ptr_atv_fix_prc:[{}/{}].".format(f_atv.ptr_atv_fix_prc.i_fix_id, f_atv.ptr_atv_fix_prc.s_fix_desc))
if (l_fix is None) or (not l_fix.v_fix_ok):
# logger
l_log = logging.getLogger("prc_dir_fixo::__ckeck_ok")
l_log.setLevel(logging.ERROR)
l_log.error(u"<E02: fixo inexistente. aeronave:[{}/{}].".format(
f_atv.i_trf_id, f_atv.s_trf_ind))
# não encontrou o fixo, força a aeronave abandonar o procedimento
abnd.abort_prc(f_atv)
# return
return
# VOR ?
if ldefs.E_VOR == l_fix.en_fix_tipo:
# calcula raio do cone de tolerância
l_fix.f_fix_rcone = f_atv.f_trf_alt_atu * math.tan(math.radians(30))
# otherwise, outro tipo de fixo
else:
# calcula raio do cone de tolerância
l_fix.f_fix_rcone = f_atv.f_trf_alt_atu * math.tan(math.radians(40))
# distância ao fixo <= raio do cone (ver DadosDinâmicos)
if f_atv.f_atv_dst_fix <= l_fix.f_fix_rcone:
# sinaliza que aeronave atingiu o ponto através raio do cone
f_cine_data.v_interceptou_fixo = True
# coloca em manual
f_atv.en_trf_fnc_ope = ldefs.E_MANUAL
# volta a fase de verificar condições
f_atv.en_atv_fase = ldefs.E_FASE_ZERO
# return interceptou o fixo
return
# calcula distância da aeronave ao fixo (x, y)
lf_dst_x = f_cine_data.f_dst_anv_fix_x**2
lf_dst_y = f_cine_data.f_dst_anv_fix_y**2
# calcula distância do "passo" da aeronave (x, y)
lf_dlt_x = f_cine_data.f_delta_x**2
lf_dlt_y = f_cine_data.f_delta_y**2
# aeronave atingiu fixo ? (distância <= passo da aeronave)
if math.sqrt(lf_dst_x + lf_dst_y) <= math.sqrt(lf_dlt_x + lf_dlt_y):
# considera que a aeronave atingiu o fixo pelas coordenadas x, y
f_cine_data.v_interceptou_fixo = True
# coloca em manual
f_atv.en_trf_fnc_ope = ldefs.E_MANUAL
# volta a fase de verificar condições
f_atv.en_atv_fase = ldefs.E_FASE_ZERO
# return interceptou o fixo
return
# calcula nova proa de demanda
f_atv.f_atv_pro_dem = cpd.calc_proa_demanda(f_cine_data.f_dst_anv_fix_x,
f_cine_data.f_dst_anv_fix_y)
# calcula sentido de curva pelo menor ângulo
scrv.sentido_curva(f_atv)
# ajusta a razão de curva da aeronave
razc.calc_razao_curva(f_atv, l_fix.f_fix_x, l_fix.f_fix_y, f_cine_data)
# nova fase de processamento
f_atv.en_atv_fase = ldefs.E_FASE_DIRFIXO
from math import sin, cos, tan, radians
n = float(
raw_input(
"Tell me a value and I'll give you the sine, cosine and tagent: > "))
sin = sin(radians(n))
cos = cos(radians(n))
tang = tan(radians(n))
print "Your sine is {:.2f}, your cosine is {:.2f} and your tangent is {:.2f}".format(
sin, cos, tang)
from math import radians, sin, cos, tan x = radians(float(input())) print(sin(x) + cos(x) + tan(x)**2)
def tan(text):
tan = math.tan(math.radians(text))
return tan
def eval_func(xi: float) ->(float): #Esta funcion recibe el punto en que se va evaluar la funcion como flotante
return (math.tan(xi) - (4*xi)) #Funcion : tan(x) -4x
def __direciona(f_atv, f_cine_data):
"""
direcionar a aeronave a um fixo específico
@param f_atv: ponteiro para struct aeronaves
@param f_cine_data: ponteiro para pilha
"""
# check input
assert f_atv
assert f_cine_data
# active flight ?
if (not f_atv.v_atv_ok) or (ldefs.E_ATIVA != f_atv.en_trf_est_atv):
# logger
l_log = logging.getLogger("prc_dir_fixo::__direciona")
l_log.setLevel(logging.ERROR)
l_log.error("<E01: aeronave não ativa.")
# cai fora...
return
# pointer to beacon & check
l_fix = f_atv.ptr_atv_fix_prc
if (l_fix is None) or (not l_fix.v_fix_ok):
# logger
l_log = logging.getLogger("prc_dir_fixo::__direciona")
l_log.setLevel(logging.ERROR)
l_log.error(u"<E02: fixo inexistente. aeronave:[{}/{}].".format(
f_atv.i_trf_id, f_atv.s_trf_ind))
# não encontrou o fixo, força a aeronave abandonar o procedimento
abnd.abort_prc(f_atv)
# return
return
# VOR ?
if ldefs.E_VOR == l_fix.en_fix_tipo:
# calcula raio do cone de tolerância do fixo
l_fix.f_fix_rcone = f_atv.f_trf_alt_atu * math.tan(math.radians(30))
# otherwise, outro tipo...
else:
# calcula raio do cone de tolerância do fixo
l_fix.f_fix_rcone = f_atv.f_trf_alt_atu * math.tan(math.radians(40))
# distância ao fixo <= raio do cone (ver DadosDinamicos)
if f_atv.f_atv_dst_fix <= l_fix.f_fix_rcone:
# sinaliza que aeronave atingiu o ponto através raio do cone
f_cine_data.v_interceptou_fixo = True
# coloca em manual
f_atv.en_trf_fnc_ope = ldefs.E_MANUAL
# volta a fase de verificar condições
f_atv.en_atv_fase = ldefs.E_FASE_ZERO
# return interceptou o fixo
return
# calcula distância da aeronave ao fixo (x, y)
lf_dst_x = f_cine_data.f_dst_anv_fix_x**2
lf_dst_y = f_cine_data.f_dst_anv_fix_y**2
# calcula distância do "passo" da aeronave (x, y)
lf_dlt_x = f_cine_data.f_delta_x**2
lf_dlt_y = f_cine_data.f_delta_y**2
# aeronave atingiu fixo ? (distância <= passo da aeronave)
if math.sqrt(lf_dst_x + lf_dst_y) <= math.sqrt(lf_dlt_x + lf_dlt_y):
# considera que a aeronave atingiu o fixo pelas coordenadas x, y
f_cine_data.v_interceptou_fixo = True
# coloca em manual
f_atv.en_trf_fnc_ope = ldefs.E_MANUAL
# volta a fase de verificar condições
f_atv.en_atv_fase = ldefs.E_FASE_ZERO
# return interceptou o fixo
return
# calcula nova proa de demanda
f_atv.f_atv_pro_dem = cpd.calc_proa_demanda(f_cine_data.f_dst_anv_fix_x,
f_cine_data.f_dst_anv_fix_y)
# verifica se mudou a proa (curvando...)
if f_atv.f_atv_pro_dem != f_atv.f_trf_pro_atu:
# bloqueia o fixo
razc.calc_razao_curva(f_atv, l_fix.f_fix_x, l_fix.f_fix_y, f_cine_data)
import colorsys
import math
from PIL import Image
img = Image.open("corgi.jpg") # must be in same folder
width, height = img.size
pixels = img.load()
#filter portion here:
for x in range(width):
for y in range(height):
r, g, b = pixels[x, y]
h, s, v = colorsys.rgb_to_hsv(r / 255, g / 255, b / 255)
# Murder this picture with maths
t = x * y
h *= math.sin(x * s) / 10
s -= math.tan(y * 10000 / (x + 1)) / 7
v -= (t & x << 5) / (t + 1)
r, g, b = colorsys.hsv_to_rgb(h, s, v)
pixels[x, y] = (int(r * 255), int(g * 255), int(b * 255))
# img.save("corgi.jpg") #you might not need to save it, show should just open image wo saving
img.show()
'sigma_err_muRp': [],
'BR': []
}
def float_to_str(x, digits=2):
tmp = ':.{:d}f'.format(digits)
tmp = ('{' + tmp + '}').format(x)
return tmp.replace('.', 'p')
for tb_exponent in np.arange(-2, 3, 1):
for tb_coeff in np.arange(0.001, 10, 1):
tb = tb_coeff * pow(10, tb_exponent)
for cba in np.arange(-0.99, 0.99, 0.02):
ta = math.tan(math.atan(tb) - math.acos(cba))
if ta < 0:
continue
sba = math.sqrt(1 - pow(cba, 2))
mhc = max(MH, MA)
m12 = math.sqrt(pow(mhc, 2) * tb / (1 + pow(tb, 2)))
x = Calc2HDM(mode='H',
sqrts=sqrts,
type=type,
tb=tb,
m12=m12,
mh=mh,
mH=MH,
mA=MA,
mhc=mhc,
sba=sba,
