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; } } }