public FootstepPlanningAction(GridTimeAction gridAction, AnotatedAnimation anim, float sp) { state = gridAction.state; cost = gridAction.cost; animInfo = anim; speed = sp; }
public override void generatePredecessors(DefaultState DcurrentState, ref List <DefaultAction> actionList) { GridTimeState currentState = DcurrentState as GridTimeState; GridTimeAction stopAction = new GridTimeAction(currentState, Vector3.zero, 0); if (!CheckTransitionCollisions(currentState, stopAction.state, false)) { if (!useTunnel || isInsideTunnel(stopAction.state)) { actionList.Add(stopAction); } } #if USE_ANGLE_MOV foreach (float angle in movAngles) { Vector3 mov = currentState.actionMov; mov = Matrix4x4.TRS(Vector3.zero, Quaternion.AngleAxis(angle, Vector3.up), new Vector3(1, 1, 1)) * mov; mov.Normalize(); #else foreach (Vector3 mov in movDirections) { #endif foreach (float speedIncr in possibleSpeedIncrements) { float newSpeed = currentState.speed + speedIncr; if (newSpeed > MAX_SPEED) { newSpeed = MAX_SPEED; } else if (newSpeed < MIN_SPEED) { newSpeed = MIN_SPEED; } GridTimeState prevState = new GridTimeState(); prevState.ComputePreviousState(currentState, mov, newSpeed); GridTimeAction newAction = new GridTimeAction(currentState, prevState); if (!CheckTransitionCollisions(currentState, newAction.state)) { // If we are not using a tunnel // or if we are using one and the stata falls inside it if (!useTunnel || isInsideTunnel(newAction.state)) { //Debug.Log(Time.time + ": grid successor generated"); actionList.Add(newAction); // we add the action as a transition } } } } //Debug.Log(transitions.Count + " grid transitions generated at time " + Time.time); }
public override void generatePredecesors(ref DefaultState DcurrentState, ref DefaultState DpreviousState, ref DefaultState DidealGoalState, ref List <DefaultAction> transitions) { GridTimeState currentState = DcurrentState as GridTimeState; GridTimeState idealGoalState = DidealGoalState as GridTimeState; GridTimeState previousState = DpreviousState as GridTimeState; GridTimeAction stopAction = new GridTimeAction(currentState, Vector3.zero, 0); transitions.Add(stopAction); #if USE_ANGLE_MOV foreach (float angle in movAngles) { Vector3 mov = currentState.actionMov; mov = Matrix4x4.TRS(Vector3.zero, Quaternion.AngleAxis(angle, Vector3.up), new Vector3(1, 1, 1)) * mov; mov.Normalize(); #else foreach (Vector3 mov in movDirections) { #endif foreach (float speedIncr in possibleSpeedIncrements) { float newSpeed = previousState.speed - speedIncr; if (newSpeed > MAX_SPEED) { newSpeed = MAX_SPEED; } else if (newSpeed < MIN_SPEED) { newSpeed = MIN_SPEED; } GridTimeAction newAction = new GridTimeAction(previousState, mov, newSpeed, 0.0f); if (!CheckTransitionCollisions(currentState, newAction.state)) { transitions.Add(newAction); } } } }
public override FootstepPlanningAction getFirstAction() { FootstepPlanningAction firstAction = null; int plannedActions = outputPlan.Count; int i = 0; bool isGridAction = false; while (i < plannedActions && firstAction == null && !isGridAction) { DefaultAction defAction = outputPlan.Pop(); firstAction = defAction as FootstepPlanningAction; if (firstAction == null) { GridPlanningAction gridAction = defAction as GridPlanningAction; if (gridAction != null) { outputPlan.Push(defAction); isGridAction = true; } else { GridTimeAction gridTimeAction = defAction as GridTimeAction; if (gridTimeAction != null) { outputPlan.Push(defAction); isGridAction = true; } } } i++; } if (firstAction != null) { currentState = firstAction.state as FootstepPlanningState; } return(firstAction); }
public override bool HasExpired() { bool expired = false; GridTimeAction lastAction = outputPlan.Last() as GridTimeAction; if (lastAction != null) { GridTimeState lastState = lastAction.state as GridTimeState; if (lastState != null) { float lastTime = lastState.time; if (Time.time > lastTime) { expired = true; } } } else { FootstepPlanningAction lastFsAction = outputPlan.Last() as FootstepPlanningAction; if (lastFsAction != null) { FootstepPlanningState lastFsState = lastFsAction.state as FootstepPlanningState; if (lastFsState != null) { float lastTime = lastFsState.time; if (Time.time > lastTime) { expired = true; } } } } return(expired); }
public override void generatePredecesors (ref DefaultState DcurrentState, ref DefaultState DpreviousState, ref DefaultState DidealGoalState, ref List<DefaultAction> transitions) { GridTimeState currentState = DcurrentState as GridTimeState; GridTimeState idealGoalState = DidealGoalState as GridTimeState; GridTimeState previousState = DpreviousState as GridTimeState; GridTimeAction stopAction = new GridTimeAction(currentState,Vector3.zero,0); transitions.Add(stopAction); #if USE_ANGLE_MOV foreach( float angle in movAngles) { Vector3 mov = currentState.actionMov; mov = Matrix4x4.TRS(Vector3.zero, Quaternion.AngleAxis(angle,Vector3.up),new Vector3(1,1,1)) * mov; mov.Normalize(); #else foreach ( Vector3 mov in movDirections ) { #endif foreach ( float speedIncr in possibleSpeedIncrements ) { float newSpeed = previousState.speed - speedIncr; if (newSpeed > MAX_SPEED) newSpeed = MAX_SPEED; else if (newSpeed < MIN_SPEED) newSpeed = MIN_SPEED; GridTimeAction newAction = new GridTimeAction(previousState,mov,newSpeed,0.0f); if (!CheckTransitionCollisions(currentState, newAction.state)) transitions.Add(newAction); } } }
override public void generateTransitions(ref DefaultState DcurrentState, ref DefaultState DpreviousState, ref DefaultState DidealGoalState, ref List<DefaultAction> transitions) { GridTimeState currentState = DcurrentState as GridTimeState; GridTimeState idealGoalState = DidealGoalState as GridTimeState; /* if (currentState == null) currentState = new GridTimeState(DcurrentState as FootstepPlanningState); if (idealGoalState == null) idealGoalState = new GridTimeState(DidealGoalState as FootstepPlanningState); */ GridTimeAction stopAction = new GridTimeAction(currentState,Vector3.zero,0); if (useTimeConstraint) { GridTimeState stopState = stopAction.state as GridTimeState; Vector3 toGoal = idealGoalState.currentPosition - stopState.currentPosition; float availableTime = idealGoalState.time - stopState.time;// + timeWindow; float necessarySpeed = toGoal.magnitude / availableTime; float timeCost = necessarySpeed - stopState.speed; bool negative = timeCost < 0; if (negative) timeCost = 0; else timeCost *= timeCost; stopAction.cost += timeCost; } if (!CheckTransitionCollisions(currentState, stopAction.state, false)) { if (!useTunnel || isInsideTunnel(stopAction.state) ) transitions.Add(stopAction); } #if USE_ANGLE_MOV foreach( float angle in movAngles) { Vector3 mov = currentState.actionMov; mov = Matrix4x4.TRS(Vector3.zero, Quaternion.AngleAxis(angle,Vector3.up),new Vector3(1,1,1)) * mov; mov.Normalize(); #else foreach ( Vector3 mov in movDirections ) { #endif foreach( float speedIncr in possibleSpeedIncrements ) { float newSpeed = currentState.speed + speedIncr; if (newSpeed > MAX_SPEED) newSpeed = MAX_SPEED; else if (newSpeed < MIN_SPEED) newSpeed = MIN_SPEED; GridTimeAction newAction = new GridTimeAction(currentState,mov,newSpeed); if (useTimeConstraint) { GridTimeState newState = newAction.state as GridTimeState; Vector3 toGoal = idealGoalState.currentPosition - newState.currentPosition; float availableTime = idealGoalState.time - newState.time; // + timeWindow; float necessarySpeed = toGoal.magnitude / availableTime; float timeCost = necessarySpeed - newState.speed; bool negative = timeCost < 0; if (negative) timeCost = 0; else timeCost *= timeCost; newAction.cost += timeCost; } if (!CheckTransitionCollisions(currentState, newAction.state)) { // If we are not using a tunnel // or if we are using one and the stata falls inside it if (!useTunnel || isInsideTunnel(newAction.state) ) { //Debug.Log(Time.time + ": grid successor generated"); transitions.Add(newAction); // we add the action as a transition } } } } //Debug.Log(transitions.Count + " grid transitions generated at time " + Time.time); }
public override void generatePredecessors(DefaultState DcurrentState, ref List<DefaultAction> actionList) { GridTimeState currentState = DcurrentState as GridTimeState; GridTimeAction stopAction = new GridTimeAction(currentState,Vector3.zero,0); if (!CheckTransitionCollisions(currentState, stopAction.state, false)) { if (!useTunnel || isInsideTunnel(stopAction.state) ) actionList.Add(stopAction); } #if USE_ANGLE_MOV foreach( float angle in movAngles) { Vector3 mov = currentState.actionMov; mov = Matrix4x4.TRS(Vector3.zero, Quaternion.AngleAxis(angle,Vector3.up),new Vector3(1,1,1)) * mov; mov.Normalize(); #else foreach ( Vector3 mov in movDirections ) { #endif foreach( float speedIncr in possibleSpeedIncrements ) { float newSpeed = currentState.speed + speedIncr; if (newSpeed > MAX_SPEED) newSpeed = MAX_SPEED; else if (newSpeed < MIN_SPEED) newSpeed = MIN_SPEED; GridTimeState prevState = new GridTimeState(); prevState.ComputePreviousState(currentState,mov,newSpeed); GridTimeAction newAction = new GridTimeAction(currentState,prevState); if (!CheckTransitionCollisions(currentState, newAction.state)) { // If we are not using a tunnel // or if we are using one and the stata falls inside it if (!useTunnel || isInsideTunnel(newAction.state) ) { //Debug.Log(Time.time + ": grid successor generated"); actionList.Add(newAction); // we add the action as a transition } } } } //Debug.Log(transitions.Count + " grid transitions generated at time " + Time.time); }
override public void generateTransitions(ref DefaultState DcurrentState, ref DefaultState DpreviousState, ref DefaultState DidealGoalState, ref List <DefaultAction> transitions) { GridTimeState currentState = DcurrentState as GridTimeState; GridTimeState idealGoalState = DidealGoalState as GridTimeState; /* * if (currentState == null) * currentState = new GridTimeState(DcurrentState as FootstepPlanningState); * * if (idealGoalState == null) * idealGoalState = new GridTimeState(DidealGoalState as FootstepPlanningState); */ GridTimeAction stopAction = new GridTimeAction(currentState, Vector3.zero, 0); if (useTimeConstraint) { GridTimeState stopState = stopAction.state as GridTimeState; Vector3 toGoal = idealGoalState.currentPosition - stopState.currentPosition; float availableTime = idealGoalState.time - stopState.time; // + timeWindow; float necessarySpeed = toGoal.magnitude / availableTime; float timeCost = necessarySpeed - stopState.speed; bool negative = timeCost < 0; if (negative) { timeCost = 0; } else { timeCost *= timeCost; } stopAction.cost += timeCost; } if (!CheckTransitionCollisions(currentState, stopAction.state, false)) { if (!useTunnel || isInsideTunnel(stopAction.state)) { transitions.Add(stopAction); } } #if USE_ANGLE_MOV foreach (float angle in movAngles) { Vector3 mov = currentState.actionMov; mov = Matrix4x4.TRS(Vector3.zero, Quaternion.AngleAxis(angle, Vector3.up), new Vector3(1, 1, 1)) * mov; mov.Normalize(); #else foreach (Vector3 mov in movDirections) { #endif foreach (float speedIncr in possibleSpeedIncrements) { float newSpeed = currentState.speed + speedIncr; if (newSpeed > MAX_SPEED) { newSpeed = MAX_SPEED; } else if (newSpeed < MIN_SPEED) { newSpeed = MIN_SPEED; } GridTimeAction newAction = new GridTimeAction(currentState, mov, newSpeed); if (useTimeConstraint) { GridTimeState newState = newAction.state as GridTimeState; Vector3 toGoal = idealGoalState.currentPosition - newState.currentPosition; float availableTime = idealGoalState.time - newState.time; // + timeWindow; float necessarySpeed = toGoal.magnitude / availableTime; float timeCost = necessarySpeed - newState.speed; bool negative = timeCost < 0; if (negative) { timeCost = 0; } else { timeCost *= timeCost; } newAction.cost += timeCost; } if (!CheckTransitionCollisions(currentState, newAction.state)) { // If we are not using a tunnel // or if we are using one and the stata falls inside it if (!useTunnel || isInsideTunnel(newAction.state)) { //Debug.Log(Time.time + ": grid successor generated"); transitions.Add(newAction); // we add the action as a transition } } } } //Debug.Log(transitions.Count + " grid transitions generated at time " + Time.time); }
//////////////////////////////////////////////////////////////////////////////////// public override FootstepPlanningAction[] GetOutputPlan() { if (outputPlan == null) { return(null); } FootstepPlanningAction[] actionList = new FootstepPlanningAction[outputPlan.Count]; int i = 0; int count = 0; // Debug.Log("OutputPlan: " + outputPlan.Count); while (i < outputPlan.Count) { DefaultAction action = outputPlan.ElementAt(i); //Debug.Log("we have an action " + i); FootstepPlanningAction fsAction = action as FootstepPlanningAction; if (fsAction != null) { actionList[count] = fsAction; count++; if (REPRODUCTION_MODE.REPRODUCTION_MODE.Equals(Reproduction.MODE.RECORD)) { storedPlan.Add(fsAction); //Debug.Log("Recording"); } } else { GridPlanningAction gridAction = action as GridPlanningAction; if (gridAction != null) { fsAction = new FootstepPlanningAction(gridAction, analyzer.GetAnotatedAnimation("Idle"), 1.5f); if (fsAction != null) { actionList[count] = fsAction; count++; if (REPRODUCTION_MODE.REPRODUCTION_MODE.Equals(Reproduction.MODE.RECORD)) { storedPlan.Add(fsAction); //Debug.Log("Recording"); } } //Debug.Log("OutputPlan at time " + Time.time + " has " + outputPlan.Count + " actions."); //Debug.Log("Grid Action added at time: " + Time.time + " at pos " + count); } else { //Debug.Log("We might add a GridTimeAction"); GridTimeAction gridTimeAction = action as GridTimeAction; if (gridTimeAction != null) { if (analyzer != null) { fsAction = new FootstepPlanningAction(gridTimeAction, analyzer.GetAnotatedAnimation("Idle"), 1.0f); } if (fsAction != null) { //Debug.Log("Adding a gridTimeAction"); actionList[count] = fsAction; count++; if (REPRODUCTION_MODE.REPRODUCTION_MODE.Equals(Reproduction.MODE.RECORD)) { storedPlan.Add(fsAction); //Debug.Log("Recording"); } } } } } i++; } return(actionList); }