/// <summary>
/// Compute the cost associated with a given state, currently only TowardWallCost is used.
/// </summary>
/// <param name="state"></param>
/// <returns>a scalar cost value</returns>
public float GetStateCost(RedirectionManager.State state)
{
float cost = TowardWallCost(state);
//Debug.LogError("!!!!!!! cost:"+cost+", pos:" + state.posReal + ", dir:" + state.dirReal);
return(cost);
}
public override List <Segment> GetNextSegments(RedirectionManager.State state)
{
List <Segment> segs = new List <Segment>();
if (this.transitions == null) // this segment has no further transition
{
this.proba = 1;
segs.Add(this);
return(segs);
}
else if (IsEndOfSegment(state))
{
int num = this.transitions.Count;
foreach (var transition in this.transitions)
{
transition.endSeg.proba = 1.0f / num;
segs.Add(transition.endSeg);
}
return(segs);
}
else
{
this.proba = 1;
segs.Add(this);
return(segs);
}
}
/// <summary>
/// To see if we need to update the current segment according to user's state.
/// Whether a user stays at current segment, or he/she will jump into the next segment if near the end of the current one.
/// </summary>
/// <param name="state"></param> the state of the environment
public void UpdateCurrentSegment(RedirectionManager.State state)
{
if (currSeg.IsEndOfSegment(state))
{
// We need to follow the Transition to the next segment
Segment nextSeg = currSeg.transitions[0].endSeg;
foreach (var trans in currSeg.transitions)
{
// return the segment with smaller id (user's path is predefined by segment id)
if (trans.endSeg.id < nextSeg.id)
{
nextSeg = trans.endSeg;
}
}
currSeg = nextSeg;
}
}
public override List <Segment> GetNextSegments(RedirectionManager.State state)
{
List <Segment> segs = new List <Segment>();
if (IsEndOfSegment(state))
{
foreach (var transition in this.transitions)
{
segs.Add(transition.endSeg);
}
return(segs);
}
else
{
segs.Add(this);
return(segs);
}
}
public void UpdateWaypoint(RedirectionManager.State userCurrState)
{
if ((userCurrState.pos - Utilities.FlattenedPos3D(this.targetWaypoint.position)).magnitude < this.DISTANCE_TO_WAYPOINT_THRESHOLD)
{
// This is the last target
if (this.waypointIterator == this.waypoints.Count - 1)
{
// Gather Summary Statistics for each episode
simulationManager.statisticsLogger.experimentResults.Add(simulationManager.statisticsLogger.GetSummaryStatistics());
this.simulationManager.EndRound();
}
else
{
this.waypointIterator++;
this.targetWaypoint.position = new Vector3(
this.waypoints[this.waypointIterator].x,
this.targetWaypoint.position.y,
this.waypoints[this.waypointIterator].y);
}
}
}
/// <summary>
/// The cost is infinite if user is outside the tracked space
/// When inside, the cost is inverse proportional to the longest walking distance described in the FORCE paper.
/// </summary>
/// <param name="state"></param>
/// <returns>a scalar cost value</returns>
private float TowardWallCost(RedirectionManager.State state)
{
// outside the tracking space
if (Mathf.Abs(state.posReal.x) >= this.redirectionManager.resetter.maxX ||
Mathf.Abs(state.posReal.z) >= this.redirectionManager.resetter.maxZ)
{
return(Mathf.Infinity);
}
else
{
Vector2 interPoint = Vector2.zero;
// check the intersection of a ray casting from user's current pos/ori with the walls of the trackedspace
for (int i = 0; i < this.redirectionManager.roomCorners.Length; i++)
{
interPoint = Utilities.GetIntersection(Utilities.FlattenedPos2D(state.posReal),
Utilities.FlattenedDir2D(state.dirReal), this.redirectionManager.roomCorners[i % 4],
this.redirectionManager.roomCorners[(i + 1) % 4]);
// if we find the intersection point
if (!interPoint.Equals(Vector2.zero))
{
break;
}
}
if (!interPoint.Equals(Vector2.zero))
{
float costLimit = 0.1f; // the max cost will be 100 if distance=0, this is to avoid 0-division
float distance = Vector2.Distance(Utilities.FlattenedPos2D(state.posReal), interPoint);
//Debug.Log("pos: " + state.posReal + ", the interpoint is: " + interPoint
// +", distance: "+distance + ", cost:"+ 10 / (distance + costLimit));
return(10 / (distance + costLimit));
}
else
{
Debug.LogError("user pos: " + state.posReal + " dir: " + state.dirReal + " Error computing intersection point");
return(Mathf.Infinity);
}
}
}
public override bool IsEndOfSegment(RedirectionManager.State currState)
{
// near the end direction
return(Vector2.Angle(this.endDir, Utilities.FlattenedPos2D(currState.dir)) <= Segment.ANGLE_THRESHOLD);
}
public override bool IsEndOfSegment(RedirectionManager.State currState)
{
return(Vector2.Distance(this.endPos, Utilities.FlattenedPos2D(currState.pos)) <= Segment.DISTANCE_THRESHOLD);
}
public abstract bool IsEndOfSegment(RedirectionManager.State currState);
public const float ANGLE_THRESHOLD = 5f; // degree
public abstract List <Segment> GetNextSegments(RedirectionManager.State state);
/// <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
});
}
}
/// <summary>
/// Get an estimate of the future state based on the current state, redirector's action and the path to be followed.
/// ATTENTION: This method is only used for simulation(planning)! A user's actual state is only updated by the tracking system.
/// </summary>
/// <param name="s"></param> the current state of the environment.
/// <param name="a"></param> action to be taken by the redirector.
/// <param name="seg"></param> the path that the user is walking on in the current stage.
public RedirectionManager.State ApplyStateUpdate(RedirectionManager.State state, Action a, Segment seg)
{
RedirectionManager.State newState = new RedirectionManager.State();
if (seg is LineSegment)
{
LineSegment segment = (LineSegment)seg;
Vector2 tangentDir = (segment.endPos - Utilities.FlattenedPos2D(state.pos)).normalized;
Vector2 delta_p = this.redirectionManager.speedReal * this.stageDuration * tangentDir;
// update virtual position and direction
newState.pos = state.pos + Utilities.UnFlatten(delta_p);
newState.dir = state.dir;
// update the real world state according to the action
if (a.type == ActionType.CURVATURE)
{
float s = delta_p.magnitude;
//Debug.Log("s=" + s);
float kr = a.gain; // the curvature gain rou(c)
float ori_0 = Vector2.SignedAngle(Vector2.right, Utilities.FlattenedDir2D(state.dirReal)) * Mathf.Deg2Rad;
//Debug.Log("angle:"+ Vector2.SignedAngle(Vector2.right, Utilities.FlattenedDir2D(state.dirReal))+" ori_0:" + ori_0);
// The new real position depends on user's real orientation
newState.posReal.x = (Mathf.Sin(ori_0 + kr * s) - Mathf.Sin(ori_0)) / kr + state.posReal.x;
newState.posReal.z = (Mathf.Cos(ori_0) - Mathf.Cos(ori_0 + kr * s)) / kr + state.posReal.z;
newState.dirReal = Utilities.UnFlatten(Utilities.RotateVector(Utilities.FlattenedDir2D(state.dirReal), s * kr * Mathf.Rad2Deg));
//Debug.Log("ppp " + newState.posReal);
//Debug.Log("ddd " + newState.dirReal);
}
else if (a.type == ActionType.ZERO)
{
newState.posReal = state.posReal + state.dirReal * this.redirectionManager.speedReal * this.stageDuration;
newState.dirReal = state.dirReal;
}
else if (a.type == ActionType.RESET)
{
newState.posReal = state.posReal - state.dirReal * this.redirectionManager.speedReal * this.stageDuration;
newState.dirReal = -state.dirReal;
}
}
else if (seg is ArcSegment)
{
ArcSegment segment = (ArcSegment)seg;
// update virtual position and direction
float s = this.redirectionManager.speedReal * this.stageDuration;
float ori_v0 = Vector2.SignedAngle(Vector2.right, Utilities.FlattenedDir2D(state.dir)) * Mathf.Deg2Rad;
newState.pos.x = (Mathf.Sin(ori_v0 + s / segment.radius) - Mathf.Sin(ori_v0)) * segment.radius + state.pos.x;
newState.pos.z = (Mathf.Cos(ori_v0) - Mathf.Cos(ori_v0 + s / segment.radius)) / segment.radius + state.pos.z;
newState.dir = Utilities.UnFlatten(Utilities.RotateVector(state.dir, s / segment.radius));
// update the real world state according to the action
float ori_r0 = Vector2.SignedAngle(Vector2.right, Utilities.FlattenedDir2D(state.dirReal)) * Mathf.Deg2Rad;
if (a.type == ActionType.CURVATURE)
{
float kr = 1 / segment.radius + a.gain; // the compound curvature gain 1/r + rou(c)
// The new real position depends on user's real orientation
newState.posReal.x = (Mathf.Sin(ori_r0 + kr * s) - Mathf.Sin(ori_r0)) / kr + state.posReal.x;
newState.posReal.z = (Mathf.Cos(ori_r0) - Mathf.Cos(ori_r0 + kr * s)) / kr + state.posReal.z;
newState.dirReal = Utilities.UnFlatten(Utilities.RotateVector(state.dirReal, s * kr));
}
else if (a.type == ActionType.ZERO)
{
newState.posReal.x = (Mathf.Sin(ori_r0 + s / segment.radius) - Mathf.Sin(ori_r0)) * segment.radius + state.posReal.x;
newState.posReal.z = (Mathf.Cos(ori_r0) - Mathf.Cos(ori_r0 + s / segment.radius)) / segment.radius + state.posReal.z;
newState.dirReal = Utilities.UnFlatten(Utilities.RotateVector(state.dirReal, s / segment.radius));
}
else if (a.type == ActionType.RESET)
{
ori_r0 = Vector2.SignedAngle(Vector2.right, Utilities.FlattenedDir2D(-state.dirReal)) * Mathf.Deg2Rad;
newState.posReal.x = (Mathf.Sin(ori_r0 + s / segment.radius) - Mathf.Sin(ori_r0)) * segment.radius + state.posReal.x;
newState.posReal.z = (Mathf.Cos(ori_r0) - Mathf.Cos(ori_r0 + s / segment.radius)) / segment.radius + state.posReal.z;
newState.dirReal = Utilities.UnFlatten(Utilities.RotateVector(-state.dirReal, s / segment.radius));
}
}
else if (seg is RotationSegment)
{
RotationSegment segment = (RotationSegment)seg;
// update virtual position and direction
newState.pos = state.pos;
float rotatedAngle = this.redirectionManager.angularSpeedReal * this.stageDuration * Mathf.Sign(segment.angle);
newState.dir = Utilities.RotateVector(state.dir, rotatedAngle);
if (a.type == ActionType.ROTATION)
{
newState.posReal = state.posReal;
rotatedAngle = rotatedAngle * a.gain;
newState.dirReal = Utilities.RotateVector(state.dirReal, rotatedAngle);
}
else if (a.type == ActionType.ZERO)
{
newState.posReal = state.posReal;
newState.dirReal = Utilities.RotateVector(state.dirReal, rotatedAngle);
}
}
return(newState);
}
