Road generateVirtualRoad(int i1, int i2) { Road r1 = connection[i1]; Road r2 = connection[i2]; if (outLaneRange[i1, i2] == null) { return(null); } int loOutLaneNum = outLaneRange[i1, i2].First; int hiOutLaneNum = outLaneRange[i1, i2].Second; int loInLaneNum = inLaneRange[i2, i1].First; int hiInLaneNum = inLaneRange[i2, i1].Second; Debug.Assert(hiOutLaneNum - loOutLaneNum == hiInLaneNum - loInLaneNum); float r1_margin = startof(r1.curve) ? r1.margin0Param : r1.margin1Param; float r2_margin = startof(r2.curve) ? r2.margin0Param : r2.margin1Param; float r1_radiOffset = 0.5f * (r1.getLaneCenterOffset(loOutLaneNum, !startof(r1.curve)) + r1.getLaneCenterOffset(hiOutLaneNum, !startof(r1.curve))); float r2_radiOffset = 0.5f * (r2.getLaneCenterOffset(loInLaneNum, startof(r2.curve)) + r2.getLaneCenterOffset(hiInLaneNum, startof(r2.curve))); Vector3 r1_endPos = r1.at(r1_margin) + r1.rightNormal(r1_margin) * r1_radiOffset; Vector3 r2_endPos = r2.at(r2_margin) + r2.rightNormal(r2_margin) * r2_radiOffset; List <string> virtualRoadLaneCfg = new List <string>(); int virtualRoadLaneCount = hiOutLaneNum - loOutLaneNum + 1; for (int i = 0; i != virtualRoadLaneCount; ++i) { virtualRoadLaneCfg.Add("lane"); if (i != virtualRoadLaneCount - 1) { virtualRoadLaneCfg.Add("dash_white"); } } Vector2 r1_direction = startof(r1.curve) ? -r1.curve.direction_2d(r1_margin) : r1.curve.direction_2d(r1_margin); Vector2 r2_direction = startof(r2.curve) ? -r2.curve.direction_2d(r2_margin) : r2.curve.direction_2d(r2_margin); if (Geometry.Parallel(r1_direction, r2_direction)) { /*TODO: perform a U turn when r1 = r2*/ if (Algebra.isRoadNodeClose(r1_endPos, r2_endPos)) { /*exact same lane config for neighbors, just go straight*/ return(null); } //return new Road(Line.TryInit(r1_endPos, r2_endPos), virtualRoadLaneCfg, _noEntity: true); return(createNoEntityRoadIfNotNull(Line.TryInit(r1_endPos, r2_endPos), virtualRoadLaneCfg)); } else { Curve l1 = Line.TryInit(Algebra.toVector2(r1_endPos), Algebra.toVector2(r1_endPos) + Algebra.InfLength * r1_direction, r1_endPos.y, r1_endPos.y); Curve l2 = Line.TryInit(Algebra.toVector2(r2_endPos), Algebra.toVector2(r2_endPos) + Algebra.InfLength * r2_direction, r2_endPos.y, r2_endPos.y); List <Vector3> intereSectionPoint = Geometry.curveIntersect(l1, l2); if (intereSectionPoint.Count == 1) { //return new Road(Bezeir.TryInit(r1_endPos, intereSectionPoint.First(), r2_endPos), virtualRoadLaneCfg, _noEntity: true); return(createNoEntityRoadIfNotNull(Bezeir.TryInit(r1_endPos, intereSectionPoint.First(), r2_endPos), virtualRoadLaneCfg)); } else { //return new Road(Line.TryInit(r1_endPos, r2_endPos), virtualRoadLaneCfg, _noEntity: true); return(createNoEntityRoadIfNotNull(Line.TryInit(r1_endPos, r2_endPos), virtualRoadLaneCfg)); } } }
// Update is called once per frame void Update() { if (pathOn != null) { float distToTravel; if (acceleration < 0f && speed < (-acceleration) * Time.deltaTime) { distToTravel = speed * speed / (2 * (-acceleration)); } else { distToTravel = speed * Time.deltaTime + 0.5f * acceleration * Time.deltaTime * Time.deltaTime; } speed += Time.deltaTime * acceleration; speed = Mathf.Max(0f, speed); distTraveledOnSeg += distToTravel; bool termination; int nextSeg, nextLane; Pair <Road, float> nextInfo = pathOn.travelAlong(currentSeg, currentParam, distToTravel, laneOn, out nextSeg, out nextLane, out termination); if (termination) { VhCtrlOfCurrentSeg.VehicleLeave(this, laneOn); stopEvent.Invoke(); Reset(); return; } Road roadOn = nextInfo.First; currentParam = nextInfo.Second; if (currentSeg != nextSeg) { VhCtrlOfCurrentSeg.VehicleLeave(this, laneOn); distTraveledOnSeg = distToTravel; laneOn = nextLane; currentSeg = nextSeg; VhCtrlOfCurrentSeg.VehicleEnter(this, laneOn); } if (!Algebra.isclose(rightOffset, 0f)) { float lateralAcc = (lateralSpeedMagnitude * lateralSpeedMagnitude > 2 * lateralMaxAcc * Mathf.Abs(rightOffset)) ? -0.98f * lateralMaxAcc : lateralMaxAcc; lateralSpeedMagnitude = Mathf.Max(lateralSpeedMagnitude + lateralAcc * Time.deltaTime, 0f); rightOffset = Mathf.Sign(rightOffset) * Mathf.Max(Mathf.Abs(rightOffset) - lateralSpeedMagnitude * Time.deltaTime, 0f); } else { lateralSpeedMagnitude = 0f; } transform.position = roadOn.at(currentParam, usebuff: true) + roadOn.rightNormal(currentParam, usebuff: true) * (roadOn.getLaneCenterOffset(laneOn, headingOfCurrentSeg) + rightOffset); transform.rotation = headingOfCurrentSeg ? Quaternion.LookRotation(roadOn.frontNormal(currentParam, usebuff: true), roadOn.upNormal(currentParam, usebuff: true)) : Quaternion.LookRotation(-roadOn.frontNormal(currentParam, usebuff: true), roadOn.upNormal(currentParam, usebuff: true)); if (rightOffset != 0f) { if (headingOfCurrentSeg) { transform.Rotate(roadOn.upNormal(currentParam, usebuff: true), -Mathf.Sign(rightOffset) * Mathf.Atan(lateralSpeedMagnitude / Mathf.Max(speed, 0.2f)) * Mathf.Rad2Deg); } else { transform.Rotate(roadOn.upNormal(currentParam, usebuff: true), Mathf.Sign(rightOffset) * Mathf.Atan(lateralSpeedMagnitude / Mathf.Max(speed, 0.2f)) * Mathf.Rad2Deg); } } wheelRotation = (wheelRotation + distToTravel / wheeRadius * Mathf.Rad2Deg) % 360; /*TODO: calculate wheel radius*/ transform.GetChild(0).GetChild(1).localRotation = transform.GetChild(0).GetChild(2).localRotation = transform.GetChild(0).GetChild(3).localRotation = transform.GetChild(0).GetChild(4).localRotation = Quaternion.Euler(wheelRotation, 0f, 0f); } }