def c_v(car_t, pt_t1, pt_o):
""" 计算当前车辆与目标点之间Cv权重(角度变化导致的权重变化)
如果car_t是新track，由于位置检测的漂移，方向飘忽不定，暂时不使用curr_v,
由curr_d代替。
"""
if False: #not car_t.is_new:
v_t = car_t.curr_v
v_t1 = (car_t.curr_xy.vec() - pt_t1.vec())/(car_t.step*car_t.interval)
dot = np.dot(v_t, v_t1)
t_modulus = modulus(v_t)
t1_modulus = modulus(v_t1)
cos_angle = dot/(t_modulus*t1_modulus) # cosinus of angle between x and y
return 0.5 + 0.5*cos_angle
else:
D = 360 - car_t.curr_d
Dt1 = 360 - pt_o
#print D, np.cos(np.radians(D)), np.sin(np.radians(D))
v_t = unit_vector(Point(np.cos(np.radians(D)), np.sin(np.radians(D))).vec())
v_t1 = None
if car_t.curr_xy.dist(pt_t1) < 20:
v_t1 = unit_vector(Point(np.cos(np.radians(Dt1)), np.sin(np.radians(Dt1))).vec())
else:
# TODO: 暂时修正为，单纯依靠检测方向
v_t1 = unit_vector(car_t.curr_xy.vec() - pt_t1.vec())
cos_angle = np.dot(v_t, v_t1) # 如果前后点不动会导致nan值
if np.isnan(cos_angle):
cos_angle = 1
# print cos_angle, v_t, v_t1
return 0.5 + 0.5*abs(cos_angle) #不管正负方向一致就行，初始的时候是这样的
def save_v(car_list, fname):
with file(fname, 'w') as f:
f.writelines(['x,y,v\n'])
for car in car_list:
if 0 != len(car.hist_xy):
f.writelines(['%s,'%car.curr_xy.X, '%s,'%car.curr_xy.Y, \
'%s\n'%(modulus(car.curr_xy.vec() - \
car.hist_xy[-1].vec())/(car.interval*car.step))])
def save_v(car_list, fname):
with file(fname, 'w') as f:
f.writelines(['x,y,v\n'])
for car in car_list:
if 0 != len(car.hist_xy):
# 计算速度,输出单位km/h
# v = dist / time_interval
v = 3.6*modulus(car.curr_xy.vec()-car.hist_xy[-1].vec())*_resolution_/(car.interval*car.step)
f.writelines(['%s,'%car.curr_xy.X, '%s,'%car.curr_xy.Y, '%s\n'%v])
def c_p(car_t, pt_t1):
""" 计算当前车辆预期结果与目标点之间Cp权重(距离变化导致的权重变化)
如果car_t是新track，需要改写
"""
#x_t_k = car_t.curr_xy.vec()
#v_t_k = car_t.curr_v
#x_t_1 = pt_t1.vec()
#k+1 = car_t.intervel
if False: #not car_t.is_new:
r_hat = modulus(car_t.curr_xy.vec() + car_t.interval * car_t.curr_v - pt_t1.vec())
return 1 - r_hat / search_r
else:
return 1 - car_t.curr_xy.dist(pt_t1)/(search_r) #/car_t.interval
