private void InitSensors()
{
left = new BoundingSphere();
right = new BoundingSphere();
left.Radius = agentObject.ObjectBoundingSphereRadius;
right.Radius = agentObject.ObjectBoundingSphereRadius;
UpdateSensors();
avoidState = ObjectAvoidanceState.GO_FORWARD;
stateNSteps = 0;
goalVisible = true;
leftSensorHit = false;
rightSensorHit = false;
wallAhead = false;
}
/// <summary>
/// State machine run that is self-contained and controls object avoidance using sensors.
/// </summary>
private void ObjectAvoidance()
{
stage.setInfo(26, String.Format("Avoidance State: {0}", avoidState.ToString()));
// Move in the current direction for n steps
if (stateNSteps != 0)
{
stateNSteps--;
return;
}
CheckSensors();
if (InWall)
{
avoidState = ObjectAvoidanceState.BACK_UP;
}
switch (avoidState)
{
default:
case ObjectAvoidanceState.GO_FORWARD:
{
if (goalVisible)
{
Vector3 goalLocation = (treasureNav != null) ? treasureNav.Translation : nextGoal.Translation;
agentObject.turnToFace(goalLocation);
}
if (leftSensorHit && wallAhead)
{
agentObject.Step = 0;
avoidState = ObjectAvoidanceState.TURN_RIGHT;
}
else if (rightSensorHit && wallAhead)
{
agentObject.Step = 0;
avoidState = ObjectAvoidanceState.TURN_LEFT;
}
break;
}
case ObjectAvoidanceState.TURN_LEFT:
{
if (rightSensorHit)
{
//Stop moving forward and turn right until left sensor is not touching the wall
agentObject.Orientation = Matrix.CreateRotationY((float)(Math.PI / 16)) * agentObject.Orientation;
agentObject.Step = 0;
}
else
{
//Turn to the left once sensor separates so that we can stay along the wall
agentObject.Orientation = Matrix.CreateRotationY((float)(-Math.PI / 16)) * agentObject.Orientation;
agentObject.Step = 1;
avoidState = ObjectAvoidanceState.FOLLOW_WALL_RIGHT;
}
break;
}
case ObjectAvoidanceState.TURN_RIGHT:
{
if (leftSensorHit)
{
//Stop moving forward and turn right until left sensor is not touching the wall
agentObject.Orientation = Matrix.CreateRotationY((float)(-Math.PI / 16)) * agentObject.Orientation;
agentObject.Step = 0;
}
else
{
//Turn to the left once sensor separates so that we can stay along the wall
agentObject.Orientation = Matrix.CreateRotationY((float)(Math.PI / 16)) * agentObject.Orientation;
agentObject.Step = 1;
avoidState = ObjectAvoidanceState.FOLLOW_WALL_LEFT;
}
break;
}
case ObjectAvoidanceState.FOLLOW_WALL_LEFT:
{
//Stay close to the wall but not in it
//If agent is angled toward wall still, on wallhit go back to turn right state.
//If agent is angled away from wall, turn back towards wall.
//If agent was angled so far away from wall that it takes two iterations of turns, that means we're at a corner.
if (wallAhead)
{
avoidState = ObjectAvoidanceState.TURN_RIGHT;
breakOut++;
if (breakOut > 60)
{
breakOut = 0;
stateNSteps = 120;
avoidState = ObjectAvoidanceState.BACK_UP;
}
}
if (!wallAhead && leftSensorHit)
{
// Walk forward
}
if (!leftSensorHit)
{
//At Corner
stateNSteps = 60;
// Go around the corner
avoidState = ObjectAvoidanceState.LEFT_CORNER;
}
break;
}
case ObjectAvoidanceState.FOLLOW_WALL_RIGHT:
{
//Stay close to the wall but not in it
//If agent is angled toward wall still, on wallhit go back to turn right state.
//If agent is angled away from wall, turn back towards wall.
//If agent was angled so far away from wall that it takes two iterations of turns, that means we're at a corner.
if (wallAhead)
{
avoidState = ObjectAvoidanceState.TURN_LEFT;
breakOut++;
if (breakOut > 60)
{
breakOut = 0;
stateNSteps = 120;
avoidState = ObjectAvoidanceState.BACK_UP;
}
}
if (!wallAhead && rightSensorHit)
{
// Walk forward
}
if (!rightSensorHit)
{
//At Corner
stateNSteps = 60;
// Go around the corner
avoidState = ObjectAvoidanceState.RIGHT_CORNER;
}
if (!goalVisible && !wallAhead && !leftSensorHit && !rightSensorHit)
{
//Can't move towards goal, no walls ahead and no sensor hits means im on the outer edge of map
breakOut++;
if (breakOut > 60)
{
//Ran into the outer walls most likely
//About face and go straight
breakOut = 0;
agentObject.Orientation = Matrix.CreateRotationY((float)(Math.PI)) * agentObject.Orientation;
avoidState = ObjectAvoidanceState.GO_FORWARD;
}
}
break;
}
case ObjectAvoidanceState.BACK_UP:
{
if (backedUp == false)
{
//Move 90 Degrees to the left and walk in that direction
agentObject.Orientation = Matrix.CreateRotationY((float)(-Math.PI / 2)) * agentObject.Orientation;
stateNSteps = 60;
backedUp = true;
}
else
{
agentObject.Orientation = Matrix.CreateRotationY((float)(-Math.PI / 2)) * agentObject.Orientation;
avoidState = ObjectAvoidanceState.GO_FORWARD;
}
break;
}
case ObjectAvoidanceState.LEFT_CORNER:
{
if (!leftSensorHit)
{
agentObject.Orientation = Matrix.CreateRotationY((float)(Math.PI / 16)) * agentObject.Orientation;
breakOut++;
if (breakOut > 60)
{
breakOut = 0;
avoidState = ObjectAvoidanceState.GO_FORWARD;
}
}
else
{
agentObject.Orientation = Matrix.CreateRotationY((float)(-Math.PI / 16)) * agentObject.Orientation;
stateNSteps = 60;
avoidState = ObjectAvoidanceState.CORNER_PASS_LEFT;
}
break;
}
case ObjectAvoidanceState.RIGHT_CORNER:
{
if (!rightSensorHit)
{
agentObject.Orientation = Matrix.CreateRotationY((float)(-Math.PI / 16)) * agentObject.Orientation;
breakOut++;
if (breakOut > 60)
{
avoidState = ObjectAvoidanceState.GO_FORWARD;
}
}
else
{
agentObject.Orientation = Matrix.CreateRotationY((float)(Math.PI / 16)) * agentObject.Orientation;
stateNSteps = 60;
avoidState = ObjectAvoidanceState.CORNER_PASS_RIGHT;
}
break;
}
case ObjectAvoidanceState.CORNER_PASS_LEFT:
{
if (goalVisible)
{
Vector3 goalLocation = (treasureNav != null) ? treasureNav.Translation : nextGoal.Translation;
agentObject.turnToFace(goalLocation);
avoidState = ObjectAvoidanceState.GO_FORWARD;
return;
}
else
{
breakOut++;
avoidState = ObjectAvoidanceState.FOLLOW_WALL_LEFT;
if (breakOut > 60)
{
//Ran into the outer walls most likely
//About face and go straight
breakOut = 0;
agentObject.Orientation = Matrix.CreateRotationY((float)(Math.PI)) * agentObject.Orientation;
avoidState = ObjectAvoidanceState.GO_FORWARD;
}
}
break;
}
case ObjectAvoidanceState.CORNER_PASS_RIGHT:
{
if (goalVisible)
{
Vector3 goalLocation = (treasureNav != null) ? treasureNav.Translation : nextGoal.Translation;
agentObject.turnToFace(goalLocation);
avoidState = ObjectAvoidanceState.GO_FORWARD;
return;
}
else
{
breakOut++;
avoidState = ObjectAvoidanceState.FOLLOW_WALL_RIGHT;
if (breakOut > 60)
{
//Ran into the outer walls most likely
//About face and go straight
breakOut = 0;
agentObject.Orientation = Matrix.CreateRotationY((float)(Math.PI)) * agentObject.Orientation;
avoidState = ObjectAvoidanceState.GO_FORWARD;
}
}
break;
}
}
}
