def v_repulse(euclid_pts, goal=None, zone_of_influence=.6, zone_of_safety=.2):
"""
Calculates a repulsive vector
"""
polar_pts = ut.pol_of_cart(euclid_pts)
#print "v_repulse: In polar", polar_pts.T
if goal != None:
polar_goal = ut.pol_of_cart(goal)
def determine_mag(pt):
if pt[0, 0] < zone_of_safety:
#print "DANGEr", pt[0,0]
return 1.0
elif pt[0, 0] < zone_of_influence:
return (zone_of_influence - pt[0, 0]) / (zone_of_influence -
zone_of_safety)
else:
return 0.0
total = 0
repulsive_vec = np.matrix([0.0, 0.0]).T
for i in range(polar_pts.shape[1]):
pt = polar_pts[:, i]
repulse = pt.copy()
if goal != None:
turn_dir = ut.best_turn_dir(polar_goal[1, 0], pt[1, 0])
#If it's in front
if np.abs(turn_dir) < np.pi:
#if it's to the right
if turn_dir > 0:
#print "RIGHT"
repulse[1, 0] = pt[1, 0] + (np.pi / 2.0)
else:
#if it's to the left
#print "LEFT"
repulse[1, 0] = pt[1, 0] - (np.pi / 2.0)
repulse[0, 0] = determine_mag(pt)
repulsive_vec = repulsive_vec + ut.cart_of_pol(repulse)
total = 1 + total
if total == 0:
return np.matrix([0.0, 0.0]).T
else:
return repulsive_vec / float(total)
def v_repulse(euclid_pts, goal=None, zone_of_influence=.6, zone_of_safety=.2): """ Calculates a repulsive vector """ polar_pts = ut.pol_of_cart(euclid_pts) #print "v_repulse: In polar", polar_pts.T if goal != None: polar_goal = ut.pol_of_cart(goal) def determine_mag(pt): if pt[0,0] < zone_of_safety: #print "DANGEr", pt[0,0] return 1.0 elif pt[0,0] < zone_of_influence: return (zone_of_influence - pt[0,0]) / (zone_of_influence - zone_of_safety) else: return 0.0 total = 0 repulsive_vec = np.matrix([0.0, 0.0]).T for i in range(polar_pts.shape[1]): pt = polar_pts[:,i] repulse = pt.copy() if goal != None: turn_dir = ut.best_turn_dir(polar_goal[1,0], pt[1,0]) #If it's in front if np.abs(turn_dir) < np.pi: #if it's to the right if turn_dir > 0: #print "RIGHT" repulse[1,0] = pt[1,0] + (np.pi/2.0) else: #if it's to the left #print "LEFT" repulse[1,0] = pt[1,0] - (np.pi/2.0) repulse[0,0] = determine_mag(pt) repulsive_vec = repulsive_vec + ut.cart_of_pol(repulse) total = 1 + total if total == 0: return np.matrix([0.0, 0.0]).T else: return repulsive_vec / float(total)
def calc(self,t):
dir_raw = ut.pol_of_cart(self.direction.val(t))
dir = dir_raw * self.slope
if self.verbosity > 0:
print "P_TurnTo_V: at", math.degrees(dir_raw[1,0]), " deg"
if self.tolerance > abs(dir_raw[1,0]):
if not self.at_loc[0]:
self.at_loc = (True, time.time())
else:
self.at_loc = (False, time.time())
if abs(dir[1,0]) < math.radians(3):
if dir[1,0] < 0:
dir[1,0] = math.radians(-3)
else:
dir[1,0] = math.radians(3)
ret = np.matrix([0, dir[1,0]]).T
if self.verbosity > 0:
print "P_TurnTo_V: returned", ret.T
return ret
def calc(self, t):
dir_raw = ut.pol_of_cart(self.direction.val(t))
dir = dir_raw * self.slope
if self.verbosity > 0:
print "P_TurnTo_V: at", math.degrees(dir_raw[1, 0]), " deg"
if self.tolerance > abs(dir_raw[1, 0]):
if not self.at_loc[0]:
self.at_loc = (True, time.time())
else:
self.at_loc = (False, time.time())
if abs(dir[1, 0]) < math.radians(3):
if dir[1, 0] < 0:
dir[1, 0] = math.radians(-3)
else:
dir[1, 0] = math.radians(3)
ret = np.matrix([0, dir[1, 0]]).T
if self.verbosity > 0:
print "P_TurnTo_V: returned", ret.T
return ret
