/// <summary>
/// This is the function called by the child thread.
/// </summary>
void ChildThreadLoop()
{
while (runningMPC)
{
if (toPause)
{
this.PauseThread();
}
// Do Update
Debug.LogWarning("Starting MPC Loop at pos: " + this.redirectionManager.currState.posReal);
var watch = System.Diagnostics.Stopwatch.StartNew(); // start a timer
MPCResult result = Plan(this.redirectionManager.currState, this.currSeg, this.planningHorizon);
this.currAction = result.bestAction;
watch.Stop(); // stop the timer
var deltaT = watch.ElapsedMilliseconds;
Debug.Log("best action: " + this.currAction.type + ", gain:" + this.currAction.gain + ", best cost: " + result.bestCost + ", duration: " + deltaT);
//Thread.Sleep(1000); // use this if you want to be more stable
}
}
public MPCResult MPCRedirect(DataRecord.Data data, DataRecord.WayPointsReal[] wayPoints, int depth)
{
MPCResult result = new MPCResult {
cost = 0, action = ActionTaken.Zero
};
if (depth == 0)
{
return(result);
}
else
{
MPCResult tempResult = new MPCResult {
};
MPCResult loopBestResult = new MPCResult {
cost = Mathf.Infinity
};
int matchNum = NearestWayPoint(data.realPosition, wayPoints);
//foreach(var wayPoint in wayPoints)
//{
// Debug.Log(wayPoint.realPosition);
//}
//loop action
for (int i = 0; i < MPCResultNum; ++i)
{
int nearestNumTemp;
float distanceCost;
float parallelCost;
float rotationAngle;
Vector3 endPosition;
Vector3 endDir;
DataRecord.WayPointsReal[] tempWayPoints;
DataRecord.Data tempPlayerData = new DataRecord.Data();
switch ((ActionTaken)i)
{
case ActionTaken.Zero:
//foreach(var wayPoint in wayPoints)
//{
// Debug.Log(wayPoint.realPosition);
//}
endPosition = ZeroAcition(wayPoints[matchNum], wayPoints[matchNum + 1], data.velocity);
//Debug.Log(endPosition);
nearestNumTemp = NearestWayPoint(endPosition, wayPoints);
distanceCost = TowardWallCost(endPosition, width, length, distanceCenterFactor);
endDir = new Vector3(wayPoints[nearestNumTemp + 1].realPosition.x - endPosition.x, 0, wayPoints[nearestNumTemp + 1].realPosition.z - endPosition.z);
parallelCost = ParallelWallCost(endDir, parallelFactor);
tempPlayerData.realPosition = endPosition;
tempPlayerData.velocity = data.velocity;
tempResult.action = ActionTaken.Zero;
tempResult.cost = distanceCost + parallelCost + MPCRedirect(tempPlayerData, wayPoints, depth - 1).cost;
break;
case ActionTaken.PositiveRotation:
if (rotationCost > loopBestResult.cost)
{
break;
}
else if (wayPoints[matchNum].turnType != WayPoint.turnType.ninetyLeft && wayPoints[matchNum].turnType != WayPoint.turnType.ninetyRight)
{
break;
}
else
{
if (wayPoints[matchNum].turnType == WayPoint.turnType.ninetyRight)
{
rotationAngle = (rotateGainEnlarge - 1) * 90;
tempWayPoints = RotateWayPoints(data.realPosition, wayPoints, rotationAngle);
endPosition = ZeroAcition(wayPoints[matchNum], tempWayPoints[matchNum + 1], data.velocity);
nearestNumTemp = NearestWayPoint(endPosition, tempWayPoints);
distanceCost = TowardWallCost(endPosition, width, length, distanceCenterFactor);
endDir = new Vector3(tempWayPoints[nearestNumTemp + 1].realPosition.x - endPosition.x, 0, tempWayPoints[nearestNumTemp + 1].realPosition.z - endPosition.z);
parallelCost = ParallelWallCost(endDir, parallelFactor);
tempPlayerData.realPosition = endPosition;
tempPlayerData.velocity = data.velocity;
tempResult.action = ActionTaken.PositiveRotation;
tempResult.cost = distanceCost + parallelCost + rotationCost + MPCRedirect(tempPlayerData, tempWayPoints, depth - 1).cost;
}
else if (wayPoints[matchNum].turnType == WayPoint.turnType.ninetyLeft)
{
rotationAngle = 360 - (rotateGainEnlarge - 1) * 90;
tempWayPoints = RotateWayPoints(data.realPosition, wayPoints, rotationAngle);
endPosition = ZeroAcition(wayPoints[matchNum], tempWayPoints[matchNum + 1], data.velocity);
nearestNumTemp = NearestWayPoint(endPosition, tempWayPoints);
distanceCost = TowardWallCost(endPosition, width, length, distanceCenterFactor);
endDir = new Vector3(tempWayPoints[nearestNumTemp + 1].realPosition.x - endPosition.x, 0, tempWayPoints[nearestNumTemp + 1].realPosition.z - endPosition.z);
parallelCost = ParallelWallCost(endDir, parallelFactor);
tempPlayerData.realPosition = endPosition;
tempPlayerData.velocity = data.velocity;
tempResult.action = ActionTaken.PositiveRotation;
tempResult.cost = rotationCost + distanceCost + parallelCost + rotationCost + MPCRedirect(tempPlayerData, tempWayPoints, depth - 1).cost;
}
}
break;
case ActionTaken.NegativeRotation:
if (rotationCost > loopBestResult.cost)
{
break;
}
else if (wayPoints[matchNum].turnType != WayPoint.turnType.ninetyLeft && wayPoints[matchNum].turnType != WayPoint.turnType.ninetyRight)
{
break;
}
else
{
if (wayPoints[matchNum].turnType == WayPoint.turnType.ninetyRight)
{
rotationAngle = 360 - (1 - rotateGainDecrease) * 90;
tempWayPoints = RotateWayPoints(data.realPosition, wayPoints, rotationAngle);
endPosition = ZeroAcition(wayPoints[matchNum], tempWayPoints[matchNum + 1], data.velocity);
nearestNumTemp = NearestWayPoint(endPosition, tempWayPoints);
distanceCost = TowardWallCost(endPosition, width, length, distanceCenterFactor);
endDir = new Vector3(tempWayPoints[nearestNumTemp + 1].realPosition.x - endPosition.x, 0, tempWayPoints[nearestNumTemp + 1].realPosition.z - endPosition.z);
parallelCost = ParallelWallCost(endDir, parallelFactor);
Debug.Log(distanceCost);
tempPlayerData.realPosition = endPosition;
tempPlayerData.velocity = data.velocity;
tempResult.action = ActionTaken.NegativeRotation;
tempResult.cost = rotationCost + distanceCost + parallelCost + MPCRedirect(tempPlayerData, tempWayPoints, depth - 1).cost;
}
else if (wayPoints[matchNum].turnType == WayPoint.turnType.ninetyLeft)
{
rotationAngle = (1 - rotateGainDecrease) * 90;
tempWayPoints = RotateWayPoints(data.realPosition, wayPoints, rotationAngle);
endPosition = ZeroAcition(wayPoints[matchNum], tempWayPoints[matchNum + 1], data.velocity);
nearestNumTemp = NearestWayPoint(endPosition, tempWayPoints);
distanceCost = TowardWallCost(endPosition, width, length, distanceCenterFactor);
endDir = new Vector3(tempWayPoints[nearestNumTemp + 1].realPosition.x - endPosition.x, 0, tempWayPoints[nearestNumTemp + 1].realPosition.z - endPosition.z);
parallelCost = ParallelWallCost(endDir, parallelFactor);
tempPlayerData.realPosition = endPosition;
tempPlayerData.velocity = data.velocity;
tempResult.action = ActionTaken.NegativeRotation;
tempResult.cost = rotationCost + distanceCost + parallelCost + MPCRedirect(tempPlayerData, tempWayPoints, depth - 1).cost;
}
}
break;
case ActionTaken.PositiveReset:
if (resetCost > loopBestResult.cost)
{
break;
}
else
{
tempWayPoints = RotateWayPoints(data.realPosition, wayPoints, 180);
endPosition = ZeroAcition(tempWayPoints[matchNum], tempWayPoints[matchNum + 1], data.velocity);
nearestNumTemp = NearestWayPoint(endPosition, tempWayPoints);
distanceCost = TowardWallCost(endPosition, width, length, distanceCenterFactor);
endDir = new Vector3(tempWayPoints[nearestNumTemp + 1].realPosition.x - endPosition.x, 0, tempWayPoints[nearestNumTemp + 1].realPosition.z - endPosition.z);
parallelCost = ParallelWallCost(endDir, parallelFactor);
tempPlayerData.realPosition = endPosition;
tempPlayerData.velocity = data.velocity;
tempResult.action = ActionTaken.PositiveReset;
tempResult.cost = distanceCost + parallelCost + MPCRedirect(tempPlayerData, tempWayPoints, depth - 1).cost + resetCost;
break;
}
//case ActionTaken.NegativeReset:
// if (resetCost > loopBestResult.cost) break;
// else break;
case ActionTaken.ClockwiseCurvature:
//Debug.Log("Here"+ loopBestResult.cost);
if (curvatureCost > loopBestResult.cost)
{
break;
}
else
{
tempWayPoints = wayPoints;
endPosition = tempWayPoints[matchNum].realPosition;
int tempMatchNum = matchNum;
nearestNumTemp = matchNum;
float deltaPos = data.velocity * timeHorizon / curvatureCalFrequence;
float deltaAngle = 360 - (deltaPos * 360 / (2 * Mathf.PI * curvatureRadius));
for (int j = 0; j < curvatureCalFrequence; j++)
{
endPosition = GeneralVector3.LineDistance(endPosition, tempWayPoints[tempMatchNum + 1].realPosition, deltaPos);
if (Vector3.Distance(endPosition, tempWayPoints[tempMatchNum + 1].realPosition) < distanceThreshlod)
{
tempMatchNum++;
nearestNumTemp = tempMatchNum;
}
tempWayPoints = RotateWayPoints(endPosition, tempWayPoints, deltaAngle);
}
distanceCost = TowardWallCost(endPosition, width, length, distanceCenterFactor);
endDir = new Vector3(tempWayPoints[nearestNumTemp + 1].realPosition.x - endPosition.x, 0, tempWayPoints[nearestNumTemp + 1].realPosition.z - endPosition.z);
parallelCost = ParallelWallCost(endDir, parallelFactor);
//Debug.Log(distanceCost);
tempPlayerData.realPosition = endPosition;
tempPlayerData.velocity = data.velocity;
tempResult.action = ActionTaken.ClockwiseCurvature;
tempResult.cost = distanceCost + parallelCost + MPCRedirect(tempPlayerData, tempWayPoints, depth - 1).cost + curvatureCost;
break;
}
case ActionTaken.CounterClockwiseCurvature:
if (curvatureCost > loopBestResult.cost)
{
break;
}
else
{
tempWayPoints = wayPoints;
endPosition = tempWayPoints[matchNum].realPosition;
int tempMatchNum = matchNum;
nearestNumTemp = matchNum;
float deltaPos = data.velocity * timeHorizon / curvatureCalFrequence;
float deltaAngle = deltaPos * 360 / (2 * Mathf.PI * curvatureRadius);
for (int j = 0; j < curvatureCalFrequence; j++)
{
endPosition = GeneralVector3.LineDistance(endPosition, tempWayPoints[tempMatchNum + 1].realPosition, deltaPos);
if (Vector3.Distance(endPosition, tempWayPoints[tempMatchNum + 1].realPosition) < distanceThreshlod)
{
tempMatchNum++;
nearestNumTemp = tempMatchNum;
}
tempWayPoints = RotateWayPoints(endPosition, tempWayPoints, deltaAngle);
}
distanceCost = TowardWallCost(endPosition, width, length, distanceCenterFactor);
endDir = new Vector3(tempWayPoints[nearestNumTemp + 1].realPosition.x - endPosition.x, 0, tempWayPoints[nearestNumTemp + 1].realPosition.z - endPosition.z);
parallelCost = ParallelWallCost(endDir, parallelFactor);
tempPlayerData.realPosition = endPosition;
tempPlayerData.velocity = data.velocity;
tempResult.action = ActionTaken.CounterClockwiseCurvature;
tempResult.cost = distanceCost + parallelCost + MPCRedirect(tempPlayerData, tempWayPoints, depth - 1).cost + curvatureCost;
break;
}
default: break;
}
if (tempResult.cost < loopBestResult.cost)
{
loopBestResult = tempResult;
}
}
//Debug.Log(loopBestResult.cost);
result = loopBestResult;
//multply declinefactor
if (depth != timeDepth)
{
result.cost *= declineFactor;
}
//Debug.Log(result.cost);
return(result);
}
}
/// <summary>
/// The main MPC K-stage forward planning, this is basically depth-first tree search with branch cutting.
/// </summary>
/// <param name="state"></param> current state
/// <param name="currSeg"></param> current segment of the user
/// <param name="depth"></param> the planning depth
/// <returns></returns>
public MPCResult Plan(RedirectionManager.State state, Segment currSeg, int depth)
{
//Debug.Log("Plan called");
if (depth == 0)
{
return new MPCResult {
bestCost = 0, bestAction = { }
}
}
;
else
{
float bestCost = Mathf.Infinity;
Action bestAction = GetZeroAction();
//Debug.Log("Plan depth="+depth+" pos="+state.posReal);
float cost;
List <Action> allowedActions = GetAllowedActions(currSeg);
//Debug.Log("GetAllowedActions");
foreach (Action a in allowedActions)
{
//Debug.Log("depth=" + depth + ", action=" + a.type+", gain="+a.gain);
cost = 0f;
if (a.cost < bestCost)
{
cost += a.cost;
List <Segment> nextSegments = currSeg.GetNextSegments(state);
//Debug.Log("GetNextSegments");
foreach (var nextSeg in nextSegments)
{
//Debug.Log("?????????? " + state.dirReal);
RedirectionManager.State nextState = ApplyStateUpdate(state, a, nextSeg);
//Debug.Log("pos="+state.posReal+" next pos="+nextState.posReal);
float stateCost = GetStateCost(nextState);
cost += stateCost * nextSeg.proba;
//Debug.Log("summed cost= " + cost + ", cost of next state: " + stateCost);
if (cost >= bestCost)
{
//Debug.Log("CUTOFF cost=" + cost + ", bestcost=" + bestCost);
break;
}
if (depth > 0)
{
MPCResult nextResult = Plan(nextState, nextSeg, depth - 1);
cost += declineFactor * nextSeg.proba * nextResult.bestCost;
//Debug.Log("Depth=" + depth + ", action: " + a.type + " gain: "+a.gain+
// ", final cost=" + cost + ", next bestcost=" + nextResult.bestCost);
}
}
if (cost < bestCost)
{
//Debug.Log("update best cost, new best=" + cost + ", old bestcost=" + bestCost+", action: "
// +bestAction.type+", gain="+bestAction.gain);
bestCost = cost;
bestAction = a;
}
}
else
{
//Debug.Log("Depth=" + depth + " action " + a.type+ ":"+a.gain + " cutoff with cost " + a.cost);
}
}
//if (depth==4)
//{
// Debug.Log("Depth=" + depth + " final best cost: " + bestCost + ", best action: " + bestAction.type
// + ", gain: " + bestAction.gain + " at pos=" + state.posReal + " dir=" + state.dirReal);
//}
return(new MPCResult {
bestAction = bestAction, bestCost = bestCost
});
}
}
