def report_icoord(self,outstream): icoordcont = [] form = "ICOOR_INTERNAL %-4s %11.6f %11.6f %11.6f %-4s %-4s %-4s\n" for i,iatm in enumerate(self.ATorder): atm = self.atms[iatm] len_i,ang_i,dih_i=0.0,180.0,0.0 #print ("ROOT:", iatm, atm.root, atm.groot ) if (i > 0): len_i = distance(self.xyz[iatm],self.xyz[atm.root]) if (i > 1): ang_i = angle(self.xyz[iatm],self.xyz[atm.root],self.xyz[atm.groot[0]]) if (i > 2): dih_i = dihedral(self.xyz[iatm],self.xyz[atm.root], self.xyz[atm.groot[0]],self.xyz[atm.groot[1]]) l = form%(atm.name,dih_i,180.0-ang_i,len_i, self.atms[atm.root].name,self.atms[atm.groot[0]].name,self.atms[atm.groot[1]].name) outstream.write(l) # virtual atms if self.option.opt.report_puckering_chi: for i,atm in enumerate(self.vatms): ring = self.rings[atm.ring_index] if ring.type != 1: continue ivrt = atm.vrt_i #print ("VROOT:", ivrt, atm.root, atm.groot) len_i = distance(self.xyz[ivrt],self.xyz[atm.root]) ang_i = angle(self.xyz[ivrt],self.xyz[atm.root],self.xyz[atm.groot[0]]) dih_i = dihedral(self.xyz[ivrt],self.xyz[atm.root], self.xyz[atm.groot[0]],self.xyz[atm.groot[1]]) l = form%(atm.name,dih_i,180.0-ang_i,len_i, self.atms[atm.root].name,self.atms[atm.groot[0]].name,self.atms[atm.groot[1]].name) outstream.write(l)
def get_orientation(self): self.ser.write('imugravity\n') line = '' while True: try: line = self.ser.readline() if 'Gravity vector (int)' in line and '[' in line and ']' in line: break except serial.serialutil.SerialException: pass s, e = line.index('['), line.index(']') grav = [int(x) for x in line[s+1:e].split()] thresh = 10 planes = [(0, 0, 1), (1, -1, 0), (1, 1, 0)] print grav for i, plane in enumerate(planes): angle = utils.angle(plane, grav) / math.pi * 180 if angle < thresh: return plane elif angle > 180 - thresh: return -plane
def get_orientation(self): self.ser.write('imugravity\n') line = '' while True: try: line = self.ser.readline() if 'Gravity vector (int)' in line and '[' in line and ']' in line: break except serial.serialutil.SerialException: pass s, e = line.index('['), line.index(']') grav = [int(x) for x in line[s + 1:e].split()] thresh = 10 planes = [(0, 0, 1), (1, -1, 0), (1, 1, 0)] print grav for i, plane in enumerate(planes): angle = utils.angle(plane, grav) / math.pi * 180 if angle < thresh: return plane elif angle > 180 - thresh: return -plane
def calculate(self, pair, original): if pair is None: return None, None, None x, y, w, h = cv2.boundingRect(pair[0]) x2, y2, w2, h2 = cv2.boundingRect(pair[1]) bounding_box_center = (x + (x2 + w2)) / 2 angle = utils.angle(constants.FOCAL_LENGTHS[self.main.results.camera], bounding_box_center, original) horizontal_distance = None field_angle = None if self.main.results.camera == 'realsense': distances = [] median_distances = [] for tape in pair: for point in tape: distances.append( self.main.display.camera_provider.get_distance( point[0][0], point[0][1])) median_distances.append(np.median(distances)) distances = [] rs_distance1 = median_distances[0] rs_distance2 = median_distances[1] if rs_distance1 and rs_distance2: rect1 = cv2.minAreaRect(pair[0]) rect2 = cv2.minAreaRect(pair[1]) points1 = cv2.boxPoints(rect1) points2 = cv2.boxPoints(rect2) point1 = min(points1, key=lambda x: x[1]) point2 = min(points2, key=lambda x: x[1]) rs_distance = (rs_distance1 + rs_distance2) / 2 pixel_width = utils.pixel_width( constants.FOCAL_LENGTHS['realsense'], constants.TARGET_SIZES['2019'] ['outer_distance_between_tapes'], rs_distance) real_pixel_width = point1[0] - point2[0] try: field_angle = math.degrees( math.acos(real_pixel_width / pixel_width)) except ValueError: field_angle = 0 if rect1[1][1] < rect2[1][0]: field_angle *= -1 owned_game_piece = self.main.nt.get_item( 'game_piece', 'cargo') if self.main.results.networktables else 'cargo' robot = self.main.results.robot if owned_game_piece and robot: try: horizontal_distance = math.sqrt((rs_distance**2) - ( (constants.HEIGHT_FROM_CARPET['camera'][robot] - constants.HEIGHT_FROM_CARPET['2019']['reflectors'] [owned_game_piece])**2)) except ValueError: pass else: horizontal_distance = rs_distance return angle, horizontal_distance, field_angle
def detect_rect(self, gray): """ Detects large rectangular shape on the image :param gray: :return: """ # get corners features = cv2.goodFeaturesToTrack(gray, 500, 0.01, 10) corners = features.squeeze() # get some number of corners closest to corresponding frame corners corner_candidates = list( map( lambda p: closest(corners, p[0], p[1], HoloDetector. NUM_CANDIDATES), ((0, 0), (0, gray.shape[0]), (gray.shape[1], gray.shape[0]), (gray.shape[1], 0)))) # check for rectangularity and get a maximum area rectangle combs = itertools.product(*corner_candidates) max_rect = None max_area = 0 for c1, c2, c3, c4 in combs: angles = [ angle(c1 - c2, c3 - c2), angle(c2 - c3, c4 - c3), angle(c1 - c4, c3 - c4) ] if np.allclose(angles, np.pi / 2, rtol=0.05): area = la.norm(c2 - c1) * la.norm(c3 - c2) if area > max_area: max_rect = [c1, c2, c3, c4] max_area = area if self.debug: self.dbg_images['corners'] = self.cur_img.copy() for c in range(4): # draw candidates if corner_candidates: list( map( lambda p: cv2.circle(self.dbg_images['corners'], tuple(p), 4, (0, 0, 255), 4), corner_candidates[c] [:HoloDetector.NUM_CANDIDATES])) # draw selected rect if max_rect: cv2.circle(self.dbg_images['corners'], tuple(max_rect[c]), 7, (0, 255, 0), 4) return max_rect, max_area
def xGs(self, **kwargs): """ Get goal attempts info for xG modelling. Returns: generator of (bq, seq, feats) -------- bg: dict, background match info (see self._background_info) seq: list, sequence of timed and located events leading to the attempt feats: dict, extracted features from the attempt """ bg = self._background_info() for attempt in self.get_attempts(**kwargs): (*attempt, (_, ga)), seq = list(attempt), [] mins, secs, team_id = ga['mins'], ga['secs'], utils.get_team_id(ga) injurytime = ga.get("injurytime_play", None) # find if assist, by whom, from where, length, angle, etc. a_id, a_x, a_y, a_dist, a_angle = None, 0., 0., 0., 0. if len(attempt) > 0: last_type, last_e = attempt[-1] if last_type == 'all_passes' and last_e['type'] == 'completed': a_id = last_e['player_id'] a_x, a_y, a_dist, a_angle = _get_assist(last_e) # add current score home, away = self.result(ga['mins'], ga['secs']) if ga['team_id'] == self.team_home['id']: attack, defend = home, away else: attack, defend = away, home # feats feats = {'team_id': ga['team_id'], 'player_id': ga['player_id'], 'is_home': ga['team_id'] == self.team_home['id'], 'headed': ga.get('headed', False), 'is_goal': ga['type'] == 'goal', 'distance': utils.euclidean( ga['end']['x'], ga['end']['y'], 100, 50), 'possession': self.possession( mins, secs, team_id, injurytime=injurytime), 'angle': utils.angle((ga['end']['x'], ga['end']['y'])), 'x': ga['end']['x'], 'y': ga['end']['y'], 'mins': ga['mins'], 'secs': ga['secs'], 'assist_x': a_x, 'assist_y': a_y, 'assist_id': a_id, 'assist_angle': a_angle, 'assist_dist': a_dist, 'attack': attack, 'defend': defend} # sequential data for ftype, e in attempt: if not utils.is_loc(e): # skip unlocated events continue seq.append({ 'x': e.get('start', e.get('loc'))['x'], 'y': e.get('start', e.get('loc'))['y'], 'end_x': e.get('end', {'x': ''})['x'], 'end_y': e.get('end', {'y': ''})['y'], 'mins': e['mins'], 'secs': e['secs'], 'ftype': ftype, 'type': e.get('type', ''), 'action_type': e.get('action_type', ''), 'player_id': e['player_id'], 'team_id': utils.get_team_id(e)}) yield bg, seq, feats
def _get_assist(pass_e): angle = 0. from_x, from_y = pass_e['start']['x'], pass_e['start']['y'] to_x, to_y = pass_e['end']['x'], pass_e['end']['y'] dist = utils.euclidean(from_x, from_y, to_x, to_y) if dist > 0: angle = utils.angle((from_x, from_y), (to_x, to_y)) return from_x, from_y, dist, angle
def set_orientation(self): src = self.rect.center dst = self.target.rect.center pos = self.rect.x, self.rect.y angle = utils.angle(src, dst) self.image = pygame.transform.rotate( game_data.projectile_images[self.type][0], angle) self.rect = pygame.Rect( pos, (self.image.get_width(), self.image.get_height()))
def sample_free(self): """ Returns a random node within the defined bounds of the graph. """ r = self.range * np.sqrt(np.random.uniform()) if self.path is None: theta = np.random.uniform() * self.alpha + (self.heading - self.alpha/2) # initial tree sample free, bounds to angle else: theta = np.random.uniform() * self.alpha + (angle(self.path[0], self.path[1]) - self.alpha/2) # trees after sample free, bounds to angle return tuple((self.x_init[0] + r * np.cos(theta), self.x_init[1] + r * np.sin(theta), self.graph.dims[2][0])) # random tuple
def main(): xyz = read(args.input) print('Successfully read %s' % args.input) # Try setting user-defined cell vectors if not args.cell: print('No user-defined cell vectors, importing vectors from input') else: print('Setting user-defined cell vectors.') xyz.set_cell(args.cell) # Write temporary .pdb file that GROMACS understands write('tmp.pdb', xyz) # Calculate box vector lengths and angles cell = xyz.get_cell() a = np.linalg.norm(cell[0]) b = np.linalg.norm(cell[1]) c = np.linalg.norm(cell[2]) alpha = angle(cell[1], cell[2]) beta = angle(cell[0], cell[2]) gamma = angle(cell[0], cell[1]) # Solvate os.system('2>/dev/null 1>&2 gmx editconf -f tmp.pdb -o tmp.gro \ -box %s %s %s -angles %s %s %s' % (a / 10., b / 10., c / 10., alpha, beta, gamma)) os.system('2>/dev/null 1>&2 gmx solvate -cp tmp.gro -cs spc216.gro \ -o tmp_solv.pdb') solv = read('tmp_solv.pdb') elems = set(solv.get_chemical_symbols()) # Wrap atoms in fractional coordinate space without breaking H2O molecules solv = pbc(xyz, solv, cell) # Convert back to .xyz write('tmp_solv.xyz', solv) # Cleanup outfile = cleanup(args.input, 'tmp_solv.xyz', elems) print('Solvated structure written to %s' % outfile)
def process_state(self,state): positions = [state['player_x'], state['player_y'], state['player_velocity_x'], state['player_velocity_y']] positions_good = state['creep_pos']['GOOD'] # Positions positions_bad = state['creep_pos']['BAD'] dist_good = state['creep_dist']['GOOD'] # Distances to player dist_bad = state['creep_dist']['BAD'] vel_good = state['creep_vel']['GOOD'] # Velocities vel_bad = state['creep_vel']['BAD'] creeps = [] # Creation of useful features from creeps for creep,dist,v in zip(positions_good, dist_good,vel_good): if(dist<self.EYES_RANGE): pos_relative_creep = np.array([creep[0]-positions[0], creep[1]-positions[1]]) vel = np.array([v[0]-positions[2],v[1]-positions[3]]) vel_relative = 1/dist*np.dot(pos_relative_creep,vel) creeps.append(("GOOD",dist, int(self.N_EYES*angle(pos_relative_creep)/360),vel_relative)) for creep,dist,v in zip(positions_bad, dist_bad, vel_bad): if(dist<self.EYES_RANGE): pos_relative_creep = np.array([creep[0]-positions[0], creep[1]-positions[1]]) vel = np.array([v[0]-positions[2],v[1]-positions[3]]) vel_relative = 1/dist*np.dot(pos_relative_creep,vel) creeps.append(("GOOD",dist, int(self.N_EYES*angle(pos_relative_creep)/360),vel_relative)) creeps = sorted(creeps,key=lambda x : x[1]) # An EYE is an angle range of 2*PI/N_EYES, each eye see the closest creep in its range if it's not too far eyes_vision = np.zeros((self.N_EYES, 4)) # Distance, is_good, is_bad, Velocity eyes_vision[:,0] = np.array([0]*self.N_EYES) for creep in creeps: if eyes_vision[creep[2],1]==0 and eyes_vision[creep[2],2]==0: eyes_vision[creep[2],0]=creep[1] eyes_vision[creep[2],1]=int(creep[0]=="GOOD") eyes_vision[creep[2],2]=int(creep[0]=="BAD") eyes_vision[creep[2],3]=creep[3] return np.array([positions[2:]+list(eyes_vision.flatten())])
def rotate_moves_about_y(moves, normal): ORDER = ["F", "L", "B", "R"] di = round(angle(Z, normal, ignore_axis=1) / np.pi * 2) + 4 res = [] for m in moves: l = m[0] ll = m[1:] if l in ORDER: move = ORDER[(ORDER.index(l) + di) % 4] + ll res.append(move) else: res.append(m) return res
def _get_up_turn(v1, v2): theta = angle(v1, v2, ignore_axis=1) theta = (np.round(theta / np.pi * 2) * 90) % 360 # print(f"{theta=}") m1 = [] if theta == 90: m1 = ["U"] elif theta == 180: m1 = ["U2"] if theta == 270: m1 = ["U'"] m3 = [neg_move(m1[0])] if len(m1) != 0 else [] return m1, m3
def _radar(self, spectrum): r = [0] * spectrum for b in self.balls: angle = utils.angle(self.agent.x, b.x, self.agent.y, b.y) distance = utils.dist(self.agent.x, b.x, self.agent.y, b.y) r[utils.sector(angle, spectrum)] += utils.scale(distance) # передаем на вход сети сигналы от стен, чтобы агент не прилипал к краю r[utils.sector(0, spectrum)] += utils.scale(self.width - self.agent.x) r[utils.sector(90, spectrum)] += utils.scale(self.height - self.agent.y) r[utils.sector(180, spectrum)] += utils.scale(self.agent.x) r[utils.sector(270, spectrum)] += utils.scale(self.agent.y) return r
def _radar(self, spectrum): r = [0] * spectrum for b in self.balls: angle = utils.angle(self.agent.x, b.x, self.agent.y, b.y) distance = utils.dist(self.agent.x, b.x, self.agent.y, b.y) r[utils.sector(angle, spectrum)] += utils.scale(distance) r[utils.sector(0, spectrum)] += utils.scale(self.width - self.agent.x) r[utils.sector(90, spectrum)] += utils.scale(self.height - self.agent.y) r[utils.sector(180, spectrum)] += utils.scale(self.agent.x) r[utils.sector(270, spectrum)] += utils.scale(self.agent.y) return r
def firstBorder(amap,neighs,node): """ Find first node on border""" vx = 0 vy = -1000 neighangles = [] #print "Node ",node.id,"neighs:",neighs[node.id] for nid in neighs[node.id]: n = amap.nodes[amap.nodesIdx[nid]] ux = n.lon-node.lon uy = n.lat-node.lat ang = angle(ux,uy,vx,vy) neighangles.append((ang,nid)) neighangles.sort(key=lambda n: n[0]) return neighangles[-1][1]
def measurements(self, frame, contours): distances = [] distance = None angle = None if contours and self.main.results.camera == 'realsense': for cnt in contours: (x, y) = utils.center(cnt) distances.append( self.main.display.camera_provider.get_distance(x, y)) distance = min(distances) closest = contours[distances.index(distance)] (x, y) = utils.center(closest) angle = utils.angle(constants.FOCAL_LENGTHS['realsense'], x, frame) if distance: cv2.putText(frame, str(int(distance * 100)), (x, y), cv2.FONT_HERSHEY_SIMPLEX, 2, (0, 0, 255), 1, cv2.LINE_AA) return distance, angle, None, None
def myBuildPathNetwork(pathnodes, world, agent=None): lines = [] ### YOUR CODE GOES BELOW HERE ### # Get world lines w_lines = world.getLines() # Get a list of all possible point combinations p_lines = list(combinations(pathnodes, r=2)) x_axis = (1, 0) edg_a = None edg_b = None # Draw lines based on raycast from each pathnode to all other nodes for line in p_lines: # line = p_lines[6] # line = p_lines[1] dif_x = line[1][0] - line[0][0] dif_y = line[1][1] - line[0][1] # Get angle of line ang = angle(x_axis, (dif_x, dif_y)) # Use angle of perpendicular to get offset for agent radius x_delt = agent.maxradius * sin(ang) y_delt = agent.maxradius * cos(ang) if dif_y >= 0: edg_a = ((line[0][0] + x_delt, line[0][1] - y_delt), (line[1][0] + x_delt, line[1][1] - y_delt)) edg_b = ((line[0][0] - x_delt, line[0][1] + y_delt), (line[1][0] - x_delt, line[1][1] + y_delt)) else: edg_a = ((line[0][0] + x_delt, line[0][1] + y_delt), (line[1][0] + x_delt, line[1][1] + y_delt)) edg_b = ((line[0][0] - x_delt, line[0][1] - y_delt), (line[1][0] - x_delt, line[1][1] - y_delt)) # Now check rayTrace for created lines # Check lines to see if agent size will cause collision during movement or at node if (not rayTraceWorld(line[0], line[1], w_lines) and not rayTraceWorld(edg_a[0], edg_a[1], w_lines) and not rayTraceWorld(edg_b[0], edg_b[1], w_lines)): # print math.degrees(ang) lines.append(line) # pygame.draw.line(world.debug, (255,0,0), edg_a[0], edg_a[1], 1) # pygame.draw.line(world.debug, (0,255,0), edg_b[0], edg_b[1], 1) ### YOUR CODE GOES ABOVE HERE ### return lines
def __find_segments(self, left, right, point): if left >= right: return None, None mid = left + (right - left) // 2 angle = utils.angle(self.points[left], point, self.points[mid]) status_left = self.__check_status(point, left) status_mid = self.__check_status(point, mid) if angle <= math.pi: if status_left == PreparataHull.PointStatus.CONCAVE: if status_mid == PreparataHull.PointStatus.CONCAVE: # case 1 return self.__find_segments(mid + 1, right, point) else: # case 2 return (left, mid), (mid + 1, right) else: if status_mid == PreparataHull.PointStatus.CONVEX: # case 3 return self.__find_segments(left, mid - 1, point) else: # case 4 if mid == left: return (left, mid), (mid + 1, right) return (mid, right), (left, mid - 1) else: if status_left == PreparataHull.PointStatus.CONVEX: if status_mid == PreparataHull.PointStatus.CONVEX: # case 5 return self.__find_segments(mid + 1, right, point) else: # case 6 return (mid + 1, right), (left, mid) else: if status_mid == PreparataHull.PointStatus.CONCAVE: # case 7 return self.__find_segments(left, mid - 1, point) else: # case 8 return (left, mid - 1), (mid, right)
def test_pspace_kfac_eigendecomposition(): """ Check KFAC eigendecomposition by comparing Mv products with v where v are the top eigenvectors. The remaining ones can be more erratic because of numerical precision """ eps = 1e-3 loader, lc, parameters, model, function, n_output = get_fullyconnect_task() generator = Jacobian(layer_collection=lc, model=model, loader=loader, function=function, n_output=n_output) M_kfac = PMatKFAC(generator) M_kfac.compute_eigendecomposition() evals, evecs = M_kfac.get_eigendecomposition() # Loop through all vectors in KFE l_to_m, _ = lc.get_layerid_module_maps(model) for l_id, layer in lc.layers.items(): for i_a in range(-3, 0): for i_g in range(-3, 0): evec_v = dict() for l_id2, layer2 in lc.layers.items(): m = l_to_m[l_id2] if l_id2 == l_id: v_a = evecs[l_id][0][:, i_a].unsqueeze(0) v_g = evecs[l_id][1][:, i_g].unsqueeze(1) evec_block = kronecker(v_g, v_a) evec_tuple = (evec_block[:, :-1].contiguous(), evec_block[:, -1].contiguous()) evec_v[l_id] = evec_tuple else: evec_v[l_id2] = (torch.zeros_like(m.weight), torch.zeros_like(m.bias)) evec_v = PVector(lc, dict_repr=evec_v) Mv = M_kfac.mv(evec_v) angle_v_Mv = angle(Mv, evec_v) assert angle_v_Mv < 1 + eps and angle_v_Mv > 1 - eps norm_mv = torch.norm(Mv.get_flat_representation()) check_ratio(evals[l_id][0][i_a] * evals[l_id][1][i_g], norm_mv)
def onBorder(amap,neighs,nodeid,lastnodeid): """ Find next node on border of an object for given last nodes""" lastnode = amap.nodes[amap.nodesIdx[lastnodeid]] node = amap.nodes[amap.nodesIdx[nodeid]] vx = lastnode.lon-node.lon vy = lastnode.lat-node.lat neighangles = [] for nid in neighs[node.id]: if nid==lastnodeid: continue #print "with ",nid n = amap.nodes[amap.nodesIdx[nid]] ux = n.lon-node.lon uy = n.lat-node.lat ang = angle(ux,uy,vx,vy) neighangles.append((ang,nid)) neighangles.sort(key=lambda n: n[0]) return neighangles[-1][1]
def second_crown(state_str): for x, z, n in [(-1, 0, -X), (1, 0, X), (0, -1, -Z), (0, 1, Z)]: pos = np.array([x, -1, z]) c_side = get_color_from_state_str(state_str, pos, n) c_down = get_color_from_state_str(state_str, pos, -Y) if "D" not in [c_side, c_down]: # Down edges # print("Down edge", c_down, c_side) # Move in front of c_side color theta = angle(n, get_normal(c_side), ignore_axis=1) theta = (np.round(theta / np.pi * 2) * 90) % 360 m1 = [] if theta == 90: m1 = ["D'"] elif theta == 180: m1 = ["D2"] if theta == 270: m1 = ["D"] # is_right if c_down is to its right to_left = np.cross(get_normal(c_side), get_normal(c_down))[1] > 0 moves = BASE_SECOND_CROWN_MOVE if not to_left: # print("to_right") moves = flip_left_right(moves) moves = rotate_moves_about_y(moves, get_normal(c_side)) return m1 + moves for x in (-1, 1): for z in (-1, 1): pos = np.array([x, 0, z]) cs1 = get_color_from_state_str(state_str, pos, x * X) cs2 = get_color_from_state_str(state_str, pos, z * Z) if cs1 != get_face_for_normal(x * X) or cs2 != get_face_for_normal( z * Z): # Wrong place / orientation # print("Edge misplaced", cs1, cs2, x, z) if x * z == -1: rotate_about = x * X else: rotate_about = z * Z moves = rotate_moves_about_y(BASE_SECOND_CROWN_MOVE, rotate_about) return moves raise ValueError("No edge for second crown, is second crown done?")
def draw_contours(filtered_contours, original): if not filtered_contours: return for cnt in filtered_contours: rect = cv2.minAreaRect(cnt) box = cv2.boxPoints(rect) box = np.int0(box) cv2.drawContours(original, [box], 0, (255, 0, 255), 7) (a, b), radius = cv2.minEnclosingCircle(box) center = int(a), int(b) cv2.circle(original, center, int(radius), (0, 0, 255), 5) rect = cv2.minAreaRect(cnt) distance = utils.distance(constants.FOCAL_LENGTHS['realsense'], constants.TARGET_SIZES['2012']['width'], max(rect[1][0], rect[1][1])) * 100 angle = utils.angle(constants.FOCAL_LENGTHS['realsense'], int(a), original) font = cv2.FONT_HERSHEY_SIMPLEX cv2.putText(original, str(int(distance)), (int(a), int(b + radius)), font, 2, (255, 255, 255), 2, cv2.LINE_AA)
def measurements(original, contours): if not contours: return None, None distances = [] for cnt in contours: points = utils.box(cnt) if not points.any(): return None, None avg_real_heights = sum(utils.power_cube.values()) / len( utils.power_cube) heights = [] for i, point in enumerate(points): x = point[0] - points[i - 1][0] y = point[1] - points[i - 1][1] height = math.hypot(x, y) heights.append(height) if len(points) == 5: max_height = max(heights) half_height = max_height / 2 heights.remove(max_height) heights.extend([half_height] * 2) avg_heights = sum(heights) / len(heights) distances.append( (avg_real_heights * constants.FOCAL_LENGTHS['lifecam']) / avg_heights) min_distance = min(distances) chosen_one = contours[distances.index(min_distance)] angle = utils.angle(constants.FOCAL_LENGTHS['lifecam'], utils.center(chosen_one)[0], original) return angle, min_distance
def measurements(self, frame, boxes): if not boxes: return None, None, None bounding_box = boxes[0]['box'] x1, y1, x2, y2 = utils.bounding_box_coords(bounding_box, frame) center = (x1 + x2) / 2 angle = utils.angle(constants.FOCAL_LENGTHS['realsense'], center, frame) horizontal_distance = None if self.main.results.camera == 'realsense': distances = [] for x, y in itertools.product(range(int(x1), int(x2), 5), range(int(y1), int(y2), 5)): distances.append( self.main.display.camera_provider.get_distance(x, y)) distance = np.median(distances) try: horizontal_distance = math.sqrt((distance**2) - ( constants.HEIGHT_FROM_CARPET['camera']['genesis'])) except ValueError: pass return angle, horizontal_distance, bounding_box
return lambda x: x * tan(theta0) - 9.81 * x**2 / 2 / (v0 * cos(theta0))**2 def x(theta, m, v0): theta = radians(theta) return (tan(theta) - m) * 2 * v0**2 * cos(theta)**2 / 9.81 d = [6, 3.6] slope = d[1] / d[0] theta0 = 42 v0 = 6 length_ramp = length(d) angle_ramp = angle(d, "degrees") my_x = x(theta0, slope, v0) y = slope * my_x magnitude = sqrt(y**2 + my_x**2) angle = degrees(atan(slope)) if __name__ == "__main__": print(length_ramp) print(angle_ramp) print(equation_425(theta0, v0)) print(x(theta0, slope, v0)) print("magnitude: ", magnitude)
from utils import length, vecadd, angle, smult from math import pi, sin, cos, radians, degrees theta1 = 35 theta2 = theta1 + 109 uplusv = smult(11, [ cos(radians(theta1)) + cos(radians(theta2)), sin(radians(theta1)) + sin(radians(theta2)) ]) if __name__=="__main__": print(uplusv) print(length(uplusv)) print(angle(uplusv, "degrees"))
def start(clientID, quads, targets, speed, proxs, path, endpos, leadfoll=False): """ Boids model main program :param clientID: ID of the VRep connection :param quads: quadrotor handles :param targets: quadrotor target handles :param speed: speed of quadrotors :param proxs: proximity sensor handles :param path: quadrotor path coordinates :param endpos: ending position of quadrotors :param leadfoll: True - leader/followers mode, False - all boids following path (default) """ # definition of constants quadsNum = len(quads) # number of quadrotors viewRange = 3 # view range of quadrotors smp = 0.2 # sampling period kS = [0.30, 2.0] # separation constants [multiplication const, power const] kC = [0.30, 0.0] # cohesion constants [multiplication const, power const] kA = [0.00, 0.0] # alignment constants [multiplication const, power const] kD = [speed, 1.0] # path following constants [multiplication const, power const] kO = [0.20, 2.0 ] # obstacle avoidance constants [multiplication const, power const] # data stream init for i in range(quadsNum): vrep.simxGetObjectPosition(clientID, quads[i], -1, vrep.simx_opmode_streaming) vrep.simxGetObjectVelocity(clientID, quads[i], vrep.simx_opmode_streaming) vrep.simxReadProximitySensor(clientID, proxs[i], vrep.simx_opmode_streaming) # variables init position = [[0 for _ in range(3)] for _ in range(quadsNum)] # position of quadrotors velocity = [[0 for _ in range(3)] for _ in range(quadsNum)] # velocity of quadrotors closest = [[0 for _ in range(3)] for _ in range(quadsNum)] # coords of closest obstacle to quads visibleQuads = [[0 for _ in range(quadsNum)] for _ in range(quadsNum)] # visible quadrotors individualTarget = [ 0 ] * quadsNum # current waypoint index for each quadrotor # get closest boid to starting point leader = 0 _, tmp = vrep.simxGetObjectPosition(clientID, quads[0], -1, vrep.simx_opmode_buffer) dist = ut.norm(ut.sub(path[1], tmp)) for i in range(1, quadsNum): _, tmp = vrep.simxGetObjectPosition(clientID, quads[i], -1, vrep.simx_opmode_buffer) nrm = ut.norm(ut.sub(path[1], tmp)) if nrm < dist: dist = nrm leader = i # main boids program t1 = 0 t2 = 0.0 finished = [False] * quadsNum count = 0 counting = False file = open('data.txt', 'wt', encoding='utf-8') while vrep.simxGetConnectionId(clientID) != -1: time.sleep(smp) separation = [[0 for _ in range(3)] for _ in range(quadsNum)] # separation force cohesion = [[0 for _ in range(3)] for _ in range(quadsNum)] # cohesion force alignment = [[0 for _ in range(3)] for _ in range(quadsNum)] # alignment force destination = [[0 for _ in range(3)] for _ in range(quadsNum)] # path following force avoidance = [[0 for _ in range(3)] for _ in range(quadsNum)] # obstacle avoidance force output = [[0 for _ in range(3)] for _ in range(quadsNum)] # output force # check if all quads finished if counting: if count >= 0: file.close() return (t2 - t1) / 1000 count += 1 else: for i in range(quadsNum): nrm = ut.norm(ut.sub(position[i][0:2], path[-1][0:2])) if nrm < 2 and not finished[i]: finished[i] = True print('Quad #' + str(i) + ' finished in ' + str((vrep.simxGetLastCmdTime(clientID) - t1) / 1000) + 's') if (leadfoll and finished[leader]) or ( not leadfoll and all(_ for _ in finished)): counting = True t2 = vrep.simxGetLastCmdTime(clientID) if endpos is None: file.close() return (t2 - t1) / 1000 # read data from VRep for i in range(quadsNum): _, position[i] = vrep.simxGetObjectPosition( clientID, quads[i], -1, vrep.simx_opmode_buffer) _, velocity[i], _ = vrep.simxGetObjectVelocity( clientID, quads[i], vrep.simx_opmode_buffer) _, res, closest[i], _, _ = vrep.simxReadProximitySensor( clientID, proxs[i], vrep.simx_opmode_buffer) if not res: closest[i] = [0, 0, 0] closest[i][2] = 0 # write into data file ct = vrep.simxGetLastCmdTime(clientID) file.write(str(ct)) for i in range(quadsNum): file.write(' ' + str(position[i][0]) + ' ' + str(position[i][1]) + ' ' + str(position[i][2])) file.write(' ' + str(velocity[i][0]) + ' ' + str(velocity[i][1]) + ' ' + str(velocity[i][2])) file.write(' ' + str(closest[i][0]) + ' ' + str(closest[i][1])) file.write('\n') # compute visible quadrotors for i in range(quadsNum): for j in range(quadsNum): if i != j: temp = ut.sub(position[i], position[j]) if ut.norm(temp) < viewRange: visibleQuads[i][j] = 1 else: visibleQuads[i][j] = 0 for i in range(quadsNum): # compute separation force for j in range(quadsNum): if i != j and visibleQuads[i][j] == 1: temp = ut.sub(position[i], position[j]) nrm = ut.norm(temp) if nrm != 0: temp = ut.mul(temp, kS[0] / (nrm**kS[1])) separation[i] = ut.add(separation[i], temp) # compute cohesion and alignment forces center = [0, 0, 0] # center of the swarm if sum(visibleQuads[i]) != 0: for j in range(quadsNum): if i != j and visibleQuads[i][j] == 1: temp = ut.mul(position[j], 1 / sum(visibleQuads[i])) center = ut.add(center, temp) temp = ut.mul(velocity[j], 1 / sum(visibleQuads[i])) alignment[i] = ut.add(alignment[i], temp) cohesion[i] = ut.sub(center, position[i]) nrm = ut.norm(cohesion[i]) if nrm != 0: cohesion[i] = ut.mul(cohesion[i], kC[0] / (nrm**kC[1])) nrm = ut.norm(alignment[i]) if nrm != 0: alignment[i] = ut.mul(alignment[i], kA[0] / (nrm**kA[1])) # compute path following force if not leadfoll or i == leader or counting: if not counting: nrm = ut.norm( ut.sub(position[i][0:2], path[individualTarget[i]][0:2])) if individualTarget[i] != 0: vec1 = ut.sub(path[individualTarget[i] - 1], path[individualTarget[i]]) vec2 = ut.sub(position[i], path[individualTarget[i]]) if individualTarget[i] < len(path) - 1 and ( nrm <= 1 or ut.angle(vec1, vec2) >= math.pi / 2): individualTarget[i] += 1 # if individualTarget[i] == 2 and min(individualTarget) == 2: # print((vrep.simxGetLastCmdTime(clientID)-tt)/1000) else: vec1 = ut.sub(path[individualTarget[i] + 1], path[individualTarget[i]]) vec2 = ut.sub(position[i], path[individualTarget[i]]) if nrm <= 1 or ut.angle(vec1, vec2) <= math.pi / 2: individualTarget[i] += 1 if t1 == 0 and min(individualTarget) == 1: t1 = vrep.simxGetLastCmdTime(clientID) # tt = vrep.simxGetLastCmdTime(clientID) destination[i] = ut.sub(path[individualTarget[i]], position[i]) else: destination[i] = ut.sub(endpos[i], position[i]) nrm = ut.norm(destination[i]) if nrm != 0: if finished[i]: destination[i] = ut.mul(destination[i], 0.1) else: destination[i] = ut.mul(destination[i], kD[0] / (nrm**kD[1])) # compute output force without obstacle avoidance output[i] = separation[i] output[i] = ut.add(output[i], cohesion[i]) output[i] = ut.add(output[i], alignment[i]) output[i] = ut.add(output[i], destination[i]) # compute obstacle avoidance force # angle = ut.angle(closest[i], output[i]) # if angle > math.pi/2+0.3: # avoidance[i] = [0, 0, 0] # else: avoidance[i] = ut.sub([0, 0, 0], closest[i]) nrm = ut.norm(avoidance[i]) if nrm != 0: avoidance[i] = ut.mul(avoidance[i], kO[0] / (nrm**kO[1])) # compute output force output[i] = ut.add(output[i], avoidance[i]) if position[i][2] < 0.5: output[i][2] = 0.05 # export output to VRep for i in range(quadsNum): vrep.simxSetObjectPosition(clientID, targets[i], quads[i], output[i], vrep.simx_opmode_streaming)
def mol2graph(self, mol): if mol is None: return None g = nx.DiGraph() # Create nodes assert len(mol.GetConformers()) == 1 geom = mol.GetConformers()[0].GetPositions() for i in range(mol.GetNumAtoms()): atom_i = mol.GetAtomWithIdx(i) g.add_node( i, a_type=atom_i.GetSymbol(), a_num=atom_i.GetAtomicNum(), acceptor=0, # 0 for placeholder donor=0, aromatic=atom_i.GetIsAromatic(), hybridization=atom_i.GetHybridization(), num_h=atom_i.GetTotalNumHs(includeNeighbors=True), # 5 more node features formal_charge=atom_i.GetFormalCharge(), explicit_valence=atom_i.GetExplicitValence(), implicit_valence=atom_i.GetImplicitValence(), num_explicit_hs=atom_i.GetNumExplicitHs(), num_radical_electrons=atom_i.GetNumRadicalElectrons(), ) # Electron donor and acceptor fdef_name = os.path.join(RDConfig.RDDataDir, 'BaseFeatures.fdef') factory = ChemicalFeatures.BuildFeatureFactory(fdef_name) feats = factory.GetFeaturesForMol(mol) for f in range(len(feats)): if feats[f].GetFamily() == 'Donor': for atom_id in feats[f].GetAtomIds(): g.nodes[atom_id]['donor'] = 1 elif feats[f].GetFamily() == 'Acceptor': for atom_id in feats[f].GetAtomIds(): g.nodes[atom_id]['acceptor'] = 1 # Read Edges for i in range(mol.GetNumAtoms()): for j in range(mol.GetNumAtoms()): e_ij = mol.GetBondBetweenAtoms(i, j) if e_ij is not None: # cal angle and area assert mol.GetNumAtoms() == len(geom) angles_ijk = [] areas_ijk = [] dists_ik = [] for neighbor in mol.GetAtomWithIdx(j).GetNeighbors(): k = neighbor.GetIdx() if mol.GetBondBetweenAtoms(j, k) is not None and i != k: vector1 = geom[j] - geom[i] vector2 = geom[k] - geom[i] angles_ijk.append(angle(vector1, vector2)) areas_ijk.append(area_triangle(vector1, vector2)) dists_ik.append(cal_dist(geom[i], geom[k])) angles_ijk = np.array( angles_ijk) if angles_ijk != [] else np.array([0.]) areas_ijk = np.array( areas_ijk) if areas_ijk != [] else np.array([0.]) dists_ik = np.array( dists_ik) if dists_ik != [] else np.array([0.]) dist_ij1 = cal_dist(geom[i], geom[j], ord=1) dist_ij2 = cal_dist(geom[i], geom[j], ord=2) g.add_edge( i, j, b_type=e_ij.GetBondType(), anglemax=angles_ijk.max(), anglesum=angles_ijk.sum(), anglemean=angles_ijk.mean(), areamax=areas_ijk.max(), areasum=areas_ijk.sum(), areamean=areas_ijk.mean(), dikmax=dists_ik.max(), diksum=dists_ik.sum(), dikmean=dists_ik.mean(), dij1=dist_ij1, dij2=dist_ij2, ) # Build pyg data node_attr = self.get_nodes(g) edge_index, edge_attr = self.get_edges(g) pos = torch.FloatTensor(geom) data = Data( x=node_attr, pos=pos, edge_index=edge_index, edge_attr=edge_attr, y=None, # None as a placeholder # name=mol.GetProp('_Name'), ) return data
def detect_fissures(img): pimg = np.array(img) limiar = utils.define_threshold(img) if limiar > 0 and limiar < 0.05: offset = 10 elif limiar > 0.05: offset = 25 pimg = utils.define_skeleton(img, 45, offset) if limiar > 0.05: pimg = utils.filtragem(pimg) runs = 30 lines = [] for _ in range(runs): lines.extend(transform.probabilistic_hough_line(pimg)) directions = np.array( [utils.angle(point1, point2) for point1, point2 in lines]) directions = np.where(directions < 0, directions + 180, directions) hist = np.histogram(directions, range=[0, 180], bins=180) sort_indexes = np.argsort(hist[0]) hist = hist[0][sort_indexes], hist[1][sort_indexes] a1, a2 = hist[1][-2:] rot_pimg1 = skimage.transform.rotate(pimg, a1 - 90, resize=True) rot_pimg2 = skimage.transform.rotate(pimg, a2 - 90, resize=True) width = 3 kernel1 = np.pad(np.ones([rot_pimg1.shape[0], width]), 1, mode='constant', constant_values=-1) kernel2 = np.pad(np.ones([rot_pimg2.shape[0], width]), 1, mode='constant', constant_values=-1) corr1 = sp.ndimage.correlate(rot_pimg1, kernel1, mode='constant') corr2 = sp.ndimage.correlate(rot_pimg2, kernel2, mode='constant') corr_rot1 = utils.cut( skimage.transform.rotate(corr1, 90 - a1, resize=True), pimg.shape) corr_rot2 = utils.cut( skimage.transform.rotate(corr2, 90 - a2, resize=True), pimg.shape) thresholds = [ filters.threshold_isodata, filters.threshold_li, filters.threshold_mean, filters.threshold_minimum, filters.threshold_otsu, filters.threshold_triangle, filters.threshold_yen ] best_fitness = -np.inf best_mask = None for thr in thresholds: binary1 = np.where(corr_rot1 > thr(corr1), 1, 0) binary2 = np.where(corr_rot2 > thr(corr2), 1, 0) selem = np.ones((5, 5)) binary_dilated1 = skimage.morphology.dilation(binary1, selem=selem) binary_dilated2 = skimage.morphology.dilation(binary2, selem=selem) mask = np.logical_or(binary_dilated1, binary_dilated2) fitness_value = fitness(mask, pimg) if fitness_value > best_fitness: best_fitness = fitness_value best_mask = mask mask = best_mask rachaduras = (1 - mask) * pimg # rachaduras_rgba = np.where(rachaduras[..., np.newaxis] == 1, [255,0,0,255], [0,0,0,0]) return rachaduras
from math import sin, radians, cos from utils import length, angle, angle2, smult y0 = 20 y = 0 theta = radians(68) t = 4.6 v0 = (9.81 * t**2 / 2 - y0) / t / sin(theta) d = v0 * cos(theta) * 4.6 v0_vec = [v0 * cos(theta), v0 * sin(theta) - 9.81 * 4.6] if __name__ == "__main__": print("distancex: ", d) print("magnitude of velocity: ", length(v0_vec)) print("angle of velocity (tan): ", angle2(v0_vec, "degrees")) print("angle of velocity (cos): ", angle(v0_vec, "degrees"))
def myCreatePathNetwork(world, agent=None): nodes = set() edges = [] polys = [] ### YOUR CODE GOES BELOW HERE ### w_lines = world.getLines() w_points = sorted(world.getPoints(), key=clockwiseangle_and_distance) # For every point, try to make a triangle with every other point in scene for a in w_points: for b in filter(lambda x, a=a: x != a, w_points): if not collidedWithNonParallel(a, b, w_lines): for c in filter(lambda x, a=a, b=b: x != a and x != b, w_points): if not collidedWithNonParallel(b, c, w_lines) \ and not collidedWithNonParallel(a, c, w_lines) \ and noPointsInPolygon((a,b,c), \ filter(lambda x, a=a, b=b, c=c: x != a and x != b and x != c, w_points)): # Valid triangle! Yay appendPolyNoDuplicates((a, b, c), polys) appendLineNoDuplicates((a, b), w_lines) appendLineNoDuplicates((a, c), w_lines) appendLineNoDuplicates((b, c), w_lines) # Ensure triangles do not get made inside objects for tri in list(polys): for obj in world.getObstacles(): tmpobj = set(obj.getPoints()) if tmpobj.issuperset(list(tri)): polys.remove(tri) poly_len = 0 # Merge triangles into convex polys while poly_len != len(polys): poly_len = len(polys) for tri in polys: expandPoly(tri, polys) x_axis = (1, 0) edg_a = None edg_b = None # Create nodes and edges using center, mid-point of lines, and corners of polygon for poly in polys: w_lines = world.getLines() l_lines = [] # Center c_node = tuple([sum(x) / len(poly) for x in zip(*poly)]) l_nodes = set() # Midpoint of lines for i in xrange(-1, len(poly) - 1): if not lineInSet(poly[i], poly[i + 1], w_lines): node_a = ((3 * poly[i][0] + poly[i + 1][0]) / 4, (3 * poly[i][1] + poly[i + 1][1]) / 4) l_nodes.add(node_a) node_b = ((poly[i][0] + 3 * poly[i + 1][0]) / 4, (poly[i][1] + 3 * poly[i + 1][1]) / 4) l_nodes.add(node_b) node_c = ((poly[i][0] + poly[i + 1][0]) / 2, (poly[i][1] + poly[i + 1][1]) / 2) l_nodes.add(node_c) l_lines += list(permutations([node_a, node_b, node_c], 2)) # Get edges nodes.add(c_node) for node in l_nodes: if rayTraceAgentDependent(c_node, node, w_lines, agent): appendLineNoDuplicates((c_node, node), edges) nodes.add(node) # Offshoots of corners # for point in w_points: # node = ((point[0] + c_node[0]) / 2, # (point[1] + c_node[1]) / 2) # l_nodes.add(node) p_lines = set(combinations(l_nodes, 2)).difference(l_lines) for line in p_lines: # if distance(line[0], line[1]) > max([distance(line[0], c_node), distance(line[1], c_node)]): # continue dif_x = line[1][0] - line[0][0] dif_y = line[1][1] - line[0][1] # Get angle of line ang = angle(x_axis, (dif_x, dif_y)) # Use angle of perpendicular to get offset for agent radius x_delt = (agent.maxradius) * sin(ang) y_delt = (agent.maxradius) * cos(ang) if dif_y >= 0: edg_a = ((line[0][0] + x_delt, line[0][1] - y_delt), (line[1][0] + x_delt, line[1][1] - y_delt)) edg_b = ((line[0][0] - x_delt, line[0][1] + y_delt), (line[1][0] - x_delt, line[1][1] + y_delt)) else: edg_a = ((line[0][0] + x_delt, line[0][1] + y_delt), (line[1][0] + x_delt, line[1][1] + y_delt)) edg_b = ((line[0][0] - x_delt, line[0][1] - y_delt), (line[1][0] - x_delt, line[1][1] - y_delt)) # Now check rayTrace for created lines # Check lines to see if agent size will cause collision during movement or at node if rayTraceAgentDependent(line[0], line[1], w_lines, agent) \ and not rayTraceWorldNoEndPoints(line[0], line[1], edges): appendLineNoDuplicates(line, edges) nodes.add(line[0]) nodes.add(line[1]) ### YOUR CODE GOES ABOVE HERE ### return nodes, edges, polys
def append_stat(self, molecule): atms = molecule.atms for atm in atms: self.atype_counts[atm.aclass] += 1 for torsclass, itors in molecule.torsionparams: i = self.torsion_params.index((torsclass, itors)) self.torsion_param_counts[i] += 1 for i, bond in enumerate(molecule.bonds): a1 = bond.atm1 a2 = bond.atm2 xyz1 = molecule.xyz[a1] xyz2 = molecule.xyz[a2] d = distance(xyz1, xyz2) bondtype = [molecule.atms[a].aclass for a in [a1, a2]] if bondtype[0] > bondtype[1]: bondtype = [bondtype[1], bondtype[0]] if bondtype not in self.bonds: self.bonds.append(bondtype) self.bond_dstr.append([]) ibond = self.bonds.index(bondtype) self.bond_dstr[ibond].append(d) for i, (a1, a2, a3) in enumerate(molecule.angles): xyz1 = molecule.xyz[a1] xyz2 = molecule.xyz[a2] xyz3 = molecule.xyz[a3] ang = angle(xyz1, xyz2, xyz3) angtype = [molecule.atms[a].aclass for a in [a1, a2, a3]] if angtype[0] > angtype[2]: angtype = [angtype[2], angtype[1], angtype[0]] if angtype not in self.angles: self.angles.append(angtype) self.angle_dstr.append([]) iang = self.angles.index(angtype) self.angle_dstr[iang].append(ang) for i, (a1, a2, a3, a4) in enumerate(molecule.torsions): xyz1 = molecule.xyz[a1] xyz2 = molecule.xyz[a2] xyz3 = molecule.xyz[a3] xyz4 = molecule.xyz[a4] torsion = dihedral(xyz1, xyz2, xyz3, xyz4) torsclass, itors = molecule.torsionparams[i] itors_tot = self.torsion_params.index((torsclass, itors)) self.torsion_dstr[itors_tot].append(torsion) torstype = (TORSION_GROUP[atms[a1].aclass], TORSION_GROUP[atms[a2].aclass], TORSION_GROUP[atms[a3].aclass], TORSION_GROUP[atms[a4].aclass]) if torstype[0] == 0 or torstype[1] == 0 or torstype[ 2] == 0 or torstype[3] == 0: continue # make sure non-redundant # by making first one having lower index than the final if torstype[0] > torstype[3]: torstype_cp = copy.copy(torstype) torstype = (torstype_cp[3], torstype_cp[2], torstype_cp[1], torstype_cp[0]) if torstype not in self.torsions: self.torsions.append(torstype) self.torsion_counts.append(0) itors = self.torsions.index(torstype) self.torsion_counts[itors] += 1