/// <summary> /// 判断换道产生的收益值,返回值[0,1] /// </summary> //TODO 现在只考虑双值,0代表不值得换道,1代表值得换道 //现在需要考虑的因素:1.换道间隙够大 // 2.间隙之前的车够快或够远 // 3.间隙之后的车够远 //TODO 对外提供收益函数的替换接口 public static float JudgeValue(OCar car, OLine line) { //目标车道不存在车辆 if (line.cars.First == null) { return(1); } OCar near = car.CarClosest(line); float PreSpeed; //间隙前车速度 float nextS; //间隙后车的行驶间距 float preS; //间隙前车的行驶间距 //near在car之后 if (OCar.judgeLocation(car, near)) { //换道之后成为头车且间隙允许换道 if (near.PreCar() == null) { preS = 10000; PreSpeed = 10000; } else { OCar nearPre = near.PreCar();//间隙前车 PreSpeed = nearPre.velocity; preS = nearPre.s - car.s; } nextS = car.s - near.s; } else//near在car之前 { OCar nearNext = near.NextCar(); PreSpeed = near.velocity; preS = near.s - car.s; if (nearNext == null) { nextS = 10000;//设立一个很大的数表示无穷 } else { nextS = car.s - nearNext.s; } } if (preS <= car.transform.localScale.z || nextS <= car.transform.lossyScale.z * 1.5) { return(0); } if (car.PreCar() == null || (PreSpeed <= car.PreCar().velocity&& preS - car.PreCar().s < car.transform.lossyScale.z)) { return(0); } return(1); }
/// <summary> /// 换道路径生成算法 /// ///TODO 该算法应该是可供二次开发的 /// </summary> public static Vector3[] CalculatePath(OCar car, OLine targetLine) { var ret = new Vector3[4]; var direction = targetLine.transform.GetChild(2).position - targetLine.transform.GetChild(0).position; var norm = Vector3.Magnitude(direction); ret[0] = car.transform.position; // ret[1] = ret[0] + car.transform.forward.normalized * 3f; ret[1] = ret[0] + direction / norm * 3f; ret[2] = ret[1] + targetLine.transform.position - car.line.transform.position; ret[3] = ret[2] + ret[1] - ret[0]; return(ret); }
//public delegate float GM(float c, float m, float l, OCar previous); /// <summary> /// GM跟驰模型 /// </summary> /// <param name="c">车辆灵敏度</param> private static float OriginGM(OCar m_car, float c, float m, float l, OCar previous) { float gm = c * Mathf.Pow(m_car.Km2m(), m) * (previous.Km2m() - m_car.Km2m()) / Mathf.Pow(previous.s - m_car.s, l); return(gm); }