// Update is called once per frame void Update() { TimerIncrease(); CalculateDeltaPar(); MovePlayer(player); //Apply Redirection switch (action) { case Redirector.ActionTaken.Zero: break; case Redirector.ActionTaken.PositiveRotation: redirector.ApplyRedirection(Redirector.RedirectorType.Rotation, player.transform, deltaDistance, deltaDiv, gain: redirector.rotateGainEnlarge); break; case Redirector.ActionTaken.NegativeRotation: redirector.ApplyRedirection(Redirector.RedirectorType.Rotation, player.transform, deltaDistance, deltaDiv, gain: redirector.rotateGainDecrease); break; case Redirector.ActionTaken.ClockwiseCurvature: redirector.ApplyRedirection(Redirector.RedirectorType.Curvature, player.transform, deltaDistance, deltaDiv, clockwise: true, radius: redirector.curvatureRadius); break; case Redirector.ActionTaken.CounterClockwiseCurvature: redirector.ApplyRedirection(Redirector.RedirectorType.Curvature, player.transform, deltaDistance, deltaDiv, clockwise: false, radius: redirector.curvatureRadius); break; default: break; } //velocity data record if ((timer - timerVelocity * velocityInterval) >= velocityInterval) { velocity = distance / velocityInterval; timerVelocity++; distance = 0; } //exponentially smoothed orientation if ((timerOrientation - timerOrientation * orientationInterval) >= orientationInterval) { //ExponentiallySmoothedSample(true); } else { //ExponentiallySmoothedSample(false); } //data record if ((timer - timeHorizon * timerDataRecord) >= timeHorizon) { data.velocity = velocity; data.virtualOrientation = player.transform.eulerAngles.y;//没有使用指数平滑 data.virtualPosition = GeneralVector3.Vector3NoHeight(playerTransform.position); Vector3 originPoint = GeneralVector3.Vector3NoHeight(Corner4.position); //Debug.Log(originPoint); Vector3 xAxis = GeneralVector3.Vector3NoHeight(Corner3.position); //Debug.Log(xAxis); Vector3 zAxis = GeneralVector3.Vector3NoHeight(Corner1.position); //Debug.Log(zAxis); data.realPosition = GeneralVector3.GetRealPoint(originPoint, xAxis, zAxis, data.virtualPosition); //Debug.Log(data.realPosition); data.realOrientation = player.transform.eulerAngles.y - Center.rotation.eulerAngles.y; timerDataRecord++; WayPointsReal[] wayPointsReals = CalWayPoint2DCoor(wayPoint.wayPoints, playerTransform, wayPoint.wayPoints.Length, data.realOrientation, data.velocity * redirector.timeDepth * timeHorizon * 2, originPoint, xAxis, zAxis); //Debug.Log(data.realPosition); //Debug.Log(wayPointsReals[0].realPosition); action = redirector.MPCRedirect(data, wayPointsReals, redirector.timeDepth).action; Debug.Log(action); } }
private WayPointsReal[] CalWayPoint2DCoor(WayPoint.WayPointSequence[] wayPoints, Transform playerPos, int length, float orientation, float routineDistance, Vector3 origin, Vector3 xAxis, Vector3 zAxis) { int nearestnum = NearestMoveableWayPoint(); int largerNum, smallerNum; if (nearestnum == length - 1) { largerNum = 0; } else { largerNum = nearestnum + 1; } if (nearestnum == 0) { smallerNum = length - 1; } else { smallerNum = nearestnum - 1; } int assumedNum = Mathf.CeilToInt(routineDistance) + 1; WayPointsReal[] resultWayPoints = new WayPointsReal[assumedNum]; float anglelarger = Vector3.Angle(GeneralVector3.Vector3NoHeight(playerPos.forward), GeneralVector3.Vector3NoHeight(wayPoints[largerNum].transform.position) - GeneralVector3.Vector3NoHeight(playerPos.position)); float anglesmaller = Vector3.Angle(GeneralVector3.Vector3NoHeight(playerPos.forward), GeneralVector3.Vector3NoHeight(wayPoints[smallerNum].transform.position) - GeneralVector3.Vector3NoHeight(playerPos.position)); if (anglelarger >= anglesmaller) { int index = smallerNum; for (int i = 0; i < assumedNum; ++i) { resultWayPoints[i].realPosition = GeneralVector3.GetRealPoint(origin, xAxis, zAxis, GeneralVector3.Vector3NoHeight(wayPoints[index].transform.position)); resultWayPoints[i].turnType = wayPoints[index].turnType; if (index == 0) { index = length - 1; } else { --index; } } } else { int index = largerNum; for (int i = 0; i < assumedNum; ++i) { resultWayPoints[i].realPosition = GeneralVector3.GetRealPoint(origin, xAxis, zAxis, GeneralVector3.Vector3NoHeight(wayPoints[index].transform.position)); resultWayPoints[i].turnType = wayPoints[index].turnType; if (index == length - 1) { index = 0; } else { ++index; } } } return(resultWayPoints); }