def electron_rep_prim(self,u,v,j,k): yp = u.exp + v.exp yq = j.exp + k.exp P = vec_add(vec_times(u.centre,u.exp/yp),vec_times(v.centre,v.exp/yp)) Q = vec_add(vec_times(j.centre,j.exp/yq),vec_times(k.centre,k.exp/yq)) AA = vec_dot(u.centre,u.centre) BB = vec_dot(v.centre,v.centre) AB = vec_dot(u.centre,v.centre) CC = vec_dot(j.centre,j.centre) DD = vec_dot(k.centre,k.centre) CD = vec_dot(j.centre,k.centre) K1 =exp((-1*u.exp * v.exp*(AA -2*AB + BB))/(yp)) K2 =exp((-1*j.exp * k.exp*(CC -2*CD + DD))/(yq)) pq = vec_minus(P,Q) PQ = vec_dot(pq,pq) n1 = (2*u.exp/pi)**0.75 n2 = (2*v.exp/pi)**0.75 n3 = (2*j.exp/pi)**0.75 n4 = (2*k.exp/pi)**0.75 E = 2*pi**2.5*K1*K2/(yp*yq*(yp+yq)**0.5) * self.f0(PQ*yp*yq/(yp+yq)) * u.coef*v.coef*j.coef*k.coef * n1*n2*n3*n4 return E
def I(self,u,v,var): #var = x,y,z ##print '--------------- I',var,' ----------------' I_sum = 0 y = u.exp + v.exp P = vec_add(scal_dot(u.centre,u.exp/y),scal_dot(v.centre,v.exp/y)) if (var == 'x'): l1=u.l l2=v.l PA = P[0]-u.centre[0] PB = P[0]-v.centre[0] elif(var=='y'): l1=u.m l2=v.m PA = P[1]-u.centre[1] PB = P[1]-v.centre[1] elif(var=='z'): l1=u.n l2=v.n PA = P[2]-u.centre[2] PB = P[2]-v.centre[2] # PA = dot(z.vec_minus(P,u.centre),z.vec_minus(P,u.centre)) # PB=dot(z.vec_minus(P,v.centre),z.vec_minus(P,v.centre)) ##print 'I',var,' looping to',(l1+l2)/2 for i in xrange((l1+l2)/2 + 1) : # #print 'i=',i I_sum+=self.f_k(u,v,2*i,PA,PB,l1,l2) * (self.double_fac(2*i-1))/((2*y)**i) * (pi/y)**0.5 return I_sum
def tick(self): # TODO beautify method (make some lines shorter) # Cancel movement, if hero is standing if self.state in [Hero.LEFT_STAND, Hero.RIGHT_STAND, Hero.UP_STAND, Hero.DOWN_STAND]: return # Otherwise: Determine direction direction = { Hero.LEFT_WALK : Hero.LEFT, Hero.RIGHT_WALK : Hero.RIGHT, Hero.UP_WALK : Hero.UP, Hero.DOWN_WALK : Hero.DOWN }[self.state] corners = self.get_corners(direction) delta = util.vec_mult(direction, self.speed) dest_tiles = [self.levelmap.pixel_pos_to_tile_pos( util.vec_add(c, delta)) for c in corners] tiletypes =\ [self.levelmap.get_tile_type(t) == LevelMap.LevelMap.FLOOR for t in dest_tiles] if reduce(lambda a, b: a and b, tiletypes): self.pos.add(delta) else: self.move_to_edge(direction)
def get_corners(self, direction): upper_left = self.pos upper_right = util.vec_add(self.pos, Vector(self.size-1, 0)) lower_left = util.vec_add(self.pos, Vector(0, self.size-1)) lower_right = util.vec_add(self.pos, Vector(self.size-1, self.size-1)) if direction == Hero.LEFT: return [upper_left, lower_left] elif direction == Hero.RIGHT: return [upper_right, lower_right] elif direction == Hero.UP: return [upper_left, upper_right] elif direction == Hero.DOWN: return [lower_left, lower_right] else: return []
def show(self, screen, offset): pos = util.vec_add(self.pos, offset) offset_pos = (pos.x, pos.y - 16) if self.state == Hero.DOWN_WALK: self.__walk_down.show(screen, pos) elif self.state == Hero.UP_WALK: self.__walk_up.show(screen, pos) elif self.state == Hero.LEFT_WALK: self.__walk_left.show(screen, pos) elif self.state == Hero.RIGHT_WALK: self.__walk_right.show(screen, pos) elif self.state == Hero.DOWN_STAND: screen.blit(self.__img, offset_pos, (4, 0, 16, 31)) elif self.state == Hero.UP_STAND: screen.blit(self.__img, offset_pos, (4, 32, 16, 31)) elif self.state == Hero.LEFT_STAND: screen.blit(self.__img, offset_pos, (4, 64, 16, 31)) elif self.state == Hero.RIGHT_STAND: screen.blit(self.__img, offset_pos, (4, 96, 16, 31))
def prim_nuclear(self,u,v,C,name): charges = {'h':1,'he':2,'c':6,'o':8} y = u.exp + v.exp #AA = my_dot(u.centre,u.centre) #BB = my_dot(v.centre,v.centre) #AB = my_dot(u.centre,v.centre) R1 = vec_minus(u.centre,v.centre) R2 = my_dot(R1,R1) P = vec_add(scal_dot(u.centre,u.exp/y),scal_dot(v.centre,v.exp/y)) PC = dist(P,C) Z_c = charges[name]#1.2266/(0.87622069) if(P[0]==C[0] and P[1]==C[1] and P[2]==C[2]): B=Z_c # #print 'P==C' else: # #print 'P!=C' B = Z_c * (pi**0.5*erf(y**0.5*PC))/(2*y**0.5 * PC) V =-1*(2*pi*exp((-1*u.exp * v.exp*(R2))/(y)))/(y)*B * u.coef*v.coef *((2*u.exp/pi)**0.75) * ((2*v.exp/pi)**0.75 ) return V
def run(self): """ Run the Vicsek IROS 2014 algorithm thread. """ # ============== Vicsek Algorithm -- Parameters =============== R0 = 12.0 # Pair potential lattice constant (Unit: m) Dp = 1.0 # Pair potential spring constant (Unit: s^-2) Cf = 18.0 # Viscous friction coefficient, or damp factor (Unit: m^2/s) Cs = 5.0 # Viscous friction coefficient for the shill agent (Unit: s^-1) Vf = 1.0 # Flocking speed for the MAS to maintain (Unit: m/s) V0 = 2.0 # Maximum tracking velocity Vmax = 3.0 # Maximum navigating velocity (the copter's total max V) Alpha = 0.6 # TRG coefficient (0 <= Alpha <= 1) Beta = 0.8 # COM coefficient (0 <= Beta <= 1) Rp = 0.5 * R0 # Upper threshold for coping with over-excitation (Short-range) Rs1 = 0.8 * R0 # Upper threshold incase division by 0 (Mid-range) Rs2 = 0.3 * R0 # Constant slope factor around R0 Tau = 0.10 # Characteristc time for reaching velocity Del_t = 0.10 # Feed-forward estimation time Ts = Del_t # Let sampling period equal to del_t Rw = 100.0 # Radius for wall (Unit: m) Rt = 1.0 # Radius for target (Unit: m) Dw = 5.0 # Transition bandwith for wall (Unit: m) Ds = 2.0 # Transition bandwith for shape (Unit: m) Dt = 1.0 # Transition bandwith for target point (Unit: m) loop_count = 1 # for calculate loop time average_time = 0 is_on_hold = False # a flag marking if the copter is holding position no_rendezvous = False # a flag marking if there's a rendezvous point while not self._stopflag: time.sleep(0.001) [self._stopflag, exit_number] = _check_terminate_condition(self._vehicle, 'Vicsek') is_on_hold = _check_satellite_low(self._xbee, is_on_hold) if is_on_hold: continue loop_begin_time = default_timer() # Flocking algorithm begins # -------------- <Part I> Repulsion and Viscous -------------- # friends = copy(self._neighbors) my_location = self._vehicle.location.global_relative_frame my_velocity = self._vehicle.velocity # attributes from Vehicle returns a newly spwaned object, no need to # `copy`. But the dictionary is mutable so it should be copied. if shared.rendezvous is None: # hold position if no rendezvous if not no_rendezvous: _log_and_broadcast( self._xbee, "IFO,%s no rendezvous." % shared.AGENT_ID) no_rendezvous = True time.sleep(0.5) continue else: no_rendezvous = False # Self-propelling term my_speed = util.vec_norm2(my_velocity) V_spp = util.vec_scal(my_velocity, Vf / my_speed) # potential term and slipping term sum_potential = [0.0, 0.0, 0.0] sum_slipping = [0.0, 0.0, 0.0] # go thru each neighbor for j in friends: # location error (form: [dNorth, dEast, dDown], unit: meters) # distance, metres (get_distance_metres is also ok) X_ij = nav.get_position_error( my_location, friends[j].location_global_relative) D_ij = util.vec_norm2( X_ij[0:2]) # only measure ground distance, ignore altitude # velocity error (form: [dVx, dVy, dVz], unit: m/s) V_ij = util.vec_sub(my_velocity, friends[j].velocity) # i. short-range repulsion, when D_ij < R0 if D_ij < R0: temp_vector = util.vec_scal(X_ij, min(Rp, R0 - D_ij) / D_ij) sum_potential = util.vec_add(sum_potential, temp_vector) # ii. mid-range viscous friction temp_vector = util.vec_scal( V_ij, 1.0 / pow(max(Rs1, D_ij - R0 + Rs2), 2)) sum_slipping = util.vec_add(sum_slipping, temp_vector) # End of for j in friends # ---- collective potential acceleration ---- A_potential = util.vec_scal(sum_potential, -Dp) A_slipping = util.vec_scal(sum_slipping, Cf) # ------------ <Part II> Global Positional Constraint -------------- # alias for the rendezvous point (a comm.WrappedData object) X_trg = shared.rendezvous.location_global_relative # i. Flocking: Shill agent wall X_tmp = nav.get_position_error(my_location, X_trg) D_tmp = util.vec_norm2(X_tmp[0:2]) # normally the X_trg cannot equal to my_location, but there is still room # to add algorithm twitches to improve robustness. # The force of the wall only applies when agents are out of the area # A_wall = Cs * bump(D_tmp, Rw, Dw) * (Vf/D_tmp*X_tmp - V_i) temp_vector = util.vec_scal(X_tmp, Vf / D_tmp) temp_vector = util.vec_sub(my_velocity, temp_vector) A_wall = util.vec_scal(temp_vector, Cs * bump_function(D_tmp, Rw, Dw)) # ii. Formation: Shape shift and CoM [X_shp, X_CoM, Rs] = get_shape_factor(my_location, friends, R0, 'grid') X_tmp = nav.get_position_error(my_location, X_shp) D_tmp = util.vec_norm2(X_tmp[0:2]) if D_tmp: # deals with divide-by-zero V_shp = util.vec_scal( X_tmp, Beta * V0 * bump_function(D_tmp, Rs, Ds) / D_tmp) else: V_shp = util.vec_scal(X_tmp, 0) # rendezvous target vector X_tmp = nav.get_position_error(X_CoM, X_trg) D_tmp = util.vec_norm2(X_tmp[0:2]) V_trg = util.vec_scal( X_tmp, Alpha * V0 * bump_function(D_tmp, Rt, Dt) / D_tmp) # normally the X_trg cannot equal to X_CoM, but there is still room # to add algorithm twitches to improve robustness # tracking vector (normalize if saturated) V_trk = util.vec_add(V_shp, V_trg) S_trk = util.vec_norm2(V_trk) if S_trk > V0: V_trk = util.vec_scal(V_trk, V0 / S_trk) # ------------ <Part III> Full Dynamic ------------ # V(t+1) = V(t) + 1/Tau*(V_spp + V_trk - V(t))*Del_t + (A_pot + A_slip + A_wall)*del_t acceleration = util.vec_add(A_wall, util.vec_add(A_potential, A_slipping)) velocity = util.vec_add(V_spp, util.vec_sub(my_velocity, V_trk)) acceleration = util.vec_scal(acceleration, Del_t) velocity = util.vec_scal(velocity, Del_t / Tau) inc_velocity = util.vec_add(acceleration, velocity) des_velocity = util.vec_add(my_velocity, inc_velocity) des_speed = util.vec_norm2(des_velocity) if des_speed > Vmax: des_velocity = util.vec_scal(des_velocity, Vmax / des_speed) loop_end_time = default_timer() # Time marker ends elapsed_time = (loop_end_time - loop_begin_time) * 1000000 # convert into us inv_count = 1.0 / loop_count # incremental averaging method average_time = ( 1.0 - inv_count) * average_time + inv_count * elapsed_time loop_count = loop_count + 1 # Send thru Mavlink nav.send_ned_velocity( self._vehicle, des_velocity[0], des_velocity[1], 0, # Vz is ignored Ts) # End of While # _log_and_broadcast( self._xbee, "IFO,%s Vicsek ended with number %d." % (shared.AGENT_ID, exit_number)) util.log_info("Average loop time: %d us" % average_time)
def read_zmatrix(self,fname): f = open(fname,'r') self.file = f; name = f.readline().strip() self.mol = Molecule() self.mol.add_atom(Atom(name.lower(),1,0,0,0,connect=0)); str = f.readline(); sp = str.split(); if (sp == []): return self.mol sp[0] = sp[0].lower() if (sp==[]): return self.mol try: bl = float(sp[2]) except: bl = self.lookup_var(sp[2]) self.mol.add_atom(Atom(sp[0],2,0,0,bl,int(sp[1]))) self.mol.add_bond(2,int(sp[1])) str = f.readline(); sp = str.split(); if (sp == []): return self.mol sp[0] = sp[0].lower() try: bl = float(sp[2]); except: bl = self.lookup_var(sp[2]) try: ba = float(sp[4]); except: ba = self.lookup_var(sp[4]) a = Atom(sp[0],3,math.sin(ba*math.pi/180.0)*bl,0,self.mol.atoms[int(sp[1]) -1].z - math.cos(ba*math.pi/180.0)*bl,int(sp[1]))# BIG CHANGE self.mol.add_atom(a) self.mol.add_bond(3,int(sp[1])) atom_number = 4; while (True): #print atom_number str = f.readline(); sp = str.split(); if (sp==[]): print 'end of matrix' break sp[0] = sp[0].lower() try: bl = float(sp[2]); except: bl = self.lookup_var(sp[2]) try: ba = float(sp[4]); except: ba = self.lookup_var(sp[4]) try: dh = float(sp[6]); except: dh = self.lookup_var(sp[6]) connect = int(sp[1]) angle_connect = int(sp[3]) dihed_connect = int(sp[5]) atoms = self.mol.atoms; #print 'connect: %i angle_connect: %i' %(connect,angle_connect) vector1 = vec_minus(atoms[connect-1].xyz,atoms[angle_connect-1].xyz ); vector2 = vec_minus(atoms[connect-1].xyz,atoms[dihed_connect-1].xyz ); norm1 = vec_cross(vector1,vector2) norm2 = vec_cross(vector1,norm1) norm1 = normalise(norm1) norm2 = normalise(norm2) norm1 =vec_times(norm1,-1*math.sin(dh*math.pi/180)) norm2 = vec_times(norm2,math.cos(dh*math.pi/180)) vector3 =vec_add(norm1,norm2) vector3 =normalise(vector3) vector3 = vec_times(vector3,bl*math.sin(ba*math.pi/180.0)) vector1 = normalise(vector1) vector1 = vec_times(vector1,bl*math.cos(ba*math.pi/180.0)) vector2 = vec_add(atoms[connect - 1].xyz,vector3) vector2 = vec_minus(vector2,vector1) a = Atom(sp[0],atom_number,vector2[0],vector2[1],vector2[2],int(sp[1])) self.mol.add_atom(a) self.mol.add_bond(atom_number,int(sp[1])) atom_number+=1; return self.mol