copilotagent.java.svn-base

這是一個JAVA語言寫的多代理人程式用來模擬飛機起飛或是降落的程式

//DEPARTURES


private void DoSendDepartureChecklist(){
	pilotState = PilotState.SENT_DEPARTURE_CHECKLIST;
	print("Sending clearance delivery msgNewDepartingFlight");
	clearanceDelivery.msgNewDepartingFlight(plane, flightNumber, departing, destinationGate, destination, this);
	stateChanged();
}
private void DoRequestClearanceForDeparture(){
	pilotState = PilotState.REQUESTED_DEPARTURE_CLEARANCE;
	print("Sending clearance delivery msgRequestingClearance");
	clearanceDelivery.msgRequestingClearance(flightNumber, destinationGate);
	stateChanged();
} 
private void DoEchoDepartureClearance(){
	pilotState = PilotState.ECHO_CLEARED_FOR_DEPARTURE;
	print("Sending clearance delivery msgConfirmClearance");
	clearanceDelivery.msgConfirmClearance(flightNumber, groundController);
	stateChanged();
}

private void DoEchoWeather(){
	pilotState = PilotState.ECHO_WEATHER;
	print("Sending clearance delivery msgConfirmWeather");
	clearanceDelivery.msgConfirmWeather(flightNumber);
	print("Sending ground control msgRequestToPushBack");
	groundController.msgRequestPushBack(flightNumber, destinationGate);
	stateChanged();
}
private void DoRequestPushBack(){
	pilotState = PilotState.ASKED_FOR_PUSH_BACK;
	print("Sending ground controller msgRequestPushBack");
	groundController.msgRequestPushBack(flightNumber, destinationGate);
	stateChanged();
}
private void DoEchoPushBack(){
	pilotState = PilotState.ECHO_PUSH_BACK;
	print("Sending ground controller msgConfirmPushBack");
	if(groundController == null) {
		System.out.println("Ground controller is null"); 
	} else {
		groundController.msgConfirmPushBack(flightNumber, departing, destinationGate);
	}
	stateChanged();
}
private void DoWaitForPushBack(){
	pilotState = PilotState.PUSH_BACK;
	print("Sending ground controller msgReadyForTaxi");
	groundController.msgReadyForTaxi(flightNumber, destinationGate.getTaxiway());////////////////////////////////HOW TO GET TAXIWAY CURRENT POSITION? USE WORLD OBJECT
	pilotState = PilotState.READY_FOR_TAXI;
	stateChanged();
}
private void DoEchoTaxiPath(){
	pilotState = PilotState.ECHO_TAXI_PATH;
	print("Sending ground controller msgConfirmTaxiTakeOffPath");
	groundController.msgConfirmTaxiTakeoffPath(flightNumber, taxiDepartingPath, localController);
	stateChanged();
}
private void DoDepartureTaxiing(){
	pilotState = PilotState.DEPARTURE_TAXIING;
	//COMPLETE TAXIING PROCEDURE
	DoFollowTaxiways(taxiDepartingPath);
	print("Sending local controller msgReadyForTakeoff");
	localController.msgReadyForTakeoff(flightNumber, departing);
	stateChanged();
}
private void DoEchoHoldForDeparture(){
	pilotState = PilotState.ECHO_HOLD_FOR_DEPARTURE;
	print("Sending local controller msgConfirmPositionAndHold");
	localController.msgConfirmPositionAndHold(flightNumber);
	stateChanged();
}
private void DoEchoClearedForLCDeparture(){
	pilotState = PilotState.ECHO_CLEARED_FOR_DEPARTURE;
	print("Sending local controller msgConfirmClearedForTakeoff");
	localController.msgConfirmClearedForTakeoff(flightNumber, departing);
	stateChanged();
}
private void DoEchoDepartureTracon(){
	pilotState = PilotState.ECHO_DEPARTURE_TRACON;
	print("Sending local controller msgConfirmDepartureControl");
	localController.msgConfirmDepartureControl(flightNumber);
	stateChanged();
}

private void DoDeparture(){
	pilotState = PilotState.TAKING_OFF;
	ActionDeparture();
	//print("Sending local controller msgConfirmDepartureControl");
	//localController.msgConfirmDepartureControl(flightNumber);
	stateChanged();
}



void DoFollowTaxiways(Collection<Taxiway> path){
    WorldVector nextPos;
    // WorldObject[] taxiArray = (WorldObject[]) path.toArray();
    /* Throwing error in GUI
    for(int x = 0; x < path.size()-1; x++){
        nextPos = GetSimilarPoint(taxiArray[x], taxiArray[x+1]);
        if(nextPos!=null)
        	MoveTo(nextPos);
    }
    */
}

void MoveTo(WorldVector moveTo){
    WorldVector newVelocity = calculateHeading(moveTo, taxiVelocity);
    plane.get_world_object_state().set_velocity(newVelocity);
    while(getDistance(plane.get_world_object_state().get_position(), moveTo) >= ((taxiVelocity/2)+1)){
        try {
			inc_velocity.acquire();
		} catch (InterruptedException e) {
			e.printStackTrace();
		}
    }
    plane.get_world_object_state().set_position(moveTo);
    plane.get_world_object_state().set_velocity(new WorldVector(0,0,0));
}

double getDistance(WorldVector currentPos, WorldVector nextPos){
    return Math.sqrt(Math.pow(currentPos.x-nextPos.x,2)+Math.pow(currentPos.y-nextPos.y,2));
}

//override
WorldVector calculateHeading(WorldVector moveTo, double speed){
    WorldVector planeVector = plane.get_world_object_state().get_position();
	WorldVector velocity = new WorldVector(moveTo.x-planeVector.x, moveTo.y-planeVector.y, moveTo.z-planeVector.z);
	velocity = setMagnitudeTo(velocity, speed);
	return velocity;
}

WorldVector GetSimilarPoint(WorldObject wo1, WorldObject wo2){
	List<WorldVector> connectionList1 = wo1.get_world_object_state().getConnections();
	List<WorldVector> connectionList2 = wo2.get_world_object_state().getConnections();
	
	for(int x=0;x<connectionList1.size();x++)
	{
		for(int y=0;y<connectionList2.size();y++)
		{
			if(connectionList1.get(x).equals(connectionList2.get(y)))
				return new WorldVector(connectionList1.get(x));
		}
	}
	return null;
}

private List<WorldVector> get_vertices_from_offset(WorldVector position, List<WorldVector> offsets) {
	List<WorldVector> vertices = new ArrayList<WorldVector>();
	for (WorldVector offset : offsets)
		vertices.add(position.plus(offset));
	return vertices;
}

private void ActionDeparture(){
	//Assumes current location is at the start of a runway
	//Move to other end of runway
	print("Moving to one end of runway");

	if (departing == null)
		print("departing null");
	if (departing.get_world_object_state() == null)
		print("wos null");
	
	List<WorldVector> vertices = get_vertices_from_offset(departing.get_world_object_state().get_position(), departing.get_world_object_state().get_vertices());
	
	plane.get_world_object_state().set_velocity(calculateHeading(true, departingVelocity, vertices));
	plane.get_world_object_state().angularPosition = new WorldVector(0,0,calculateAngle(plane.get_world_object_state().get_velocity().x, plane.get_world_object_state().get_velocity().y));
	while(!checkDistanceToRunway(plane.get_world_object_state().get_position())){
		print(plane.get_world_object_state().get_position().toString());
		try {
			Thread.sleep(500);
		} catch (InterruptedException e) {
			e.printStackTrace();
		}
	}
	plane.get_world_object_state().set_position(runwayPosition);
	

	print("Plane taking off\n"+plane.get_world_object_state().get_position().toString());
	plane.get_world_object_state().set_velocity(calculateHeading(false, departingVelocity, vertices));
	
	//plane.get_world_object_state().set_velocity(moveAcrossRunway());
	WorldVector myPosition = plane.get_world_object_state().get_position();
	WorldVector velocity = plane.get_world_object_state().get_velocity();
	
	double old_distance_to_end = Math.abs(runwayPosition.x-myPosition.x);
	double current_dist_to_end = Math.abs(runwayPosition.x-myPosition.x);
	while(current_dist_to_end <= old_distance_to_end){
		myPosition = plane.get_world_object_state().get_position();
		old_distance_to_end = current_dist_to_end;
		current_dist_to_end = Math.abs(runwayPosition.x-myPosition.x);
		print(plane.get_world_object_state().get_position().toString());
		//Greater than 10 away from end of runway
		//Keep on increasing velocity until reached nearly the end of the runway
		 velocity = alterMagnitude(velocity, 1.2);
		 print("altered velocity is " + velocity.toString());
		 plane.get_world_object_state().set_velocity(velocity);
		 plane.get_world_object_state().angularPosition = new WorldVector(0,0,calculateAngle(velocity.x, velocity.y)); 
		 try {
			inc_velocity.acquire();
		} catch (InterruptedException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
	}
	print("stop increasing velocity");
	//Follow checkpoints
    //Collection<Waypoint> departureProcedure;
	/*
    for(int w=0;w<departureProcedure.size();w++){
    	//go to departureProcedure.get(w)
    	goToWaypoint(departureProcedure.get(w).get_world_object_state().position);
    }*/
}

private void goToWaypoint(WorldVector waypointPosition){
	//WorldVector velocity = new WorldVector(closest.x-planeVector.x, closest.y-planeVector.y, closest.z-planeVector.z);
	
}

/*
private WorldVector moveAcrossRunway(){
	//Creates midpoints between two points with same x coordinate
	List<WorldVector> midpoints = get_midpoints(departing.get_world_object_state().get_vertices());
	
	
	WorldVector planeVector = plane.get_world_object_state().get_position();
	WorldVector midpoint = get_midpoint(midpoints, false);

	runwayPosition = midpoint;
	return new WorldVector(midpoint.x-planeVector.x, 0, 0);
}
*/


double calculateAngle(double x, double y){
	if(x==0 && y>=0)
		return 3*Math.PI/2;
	else if(x==0 && y<0)
		return Math.PI/2;
	else if(x>0 && y>=0)
		return 2*Math.PI - Math.atan(y/x);
	else if(x<0 && y>=0)
		return Math.PI-Math.atan(y/x);
	else if(x<0 && y<0) // correct
		return Math.PI-Math.atan(y/x);
	else if(x>0 && y<0) // correct
		return 2*Math.PI - Math.atan(y/x);
		
	else
		return 0;
	
}

private WorldVector calculateHeading(boolean trueIfClosest, double velocity_multiplier, List<WorldVector> vertices){
	
	List<WorldVector> midpoints = get_midpoints(vertices);
	
	for (WorldVector vertex : vertices)
		print("vertex " + vertex.toString());
	
	WorldVector midpoint = get_midpoint(midpoints, trueIfClosest);
	
	runwayPosition = midpoint;
	
	WorldVector planeVector = plane.get_world_object_state().get_position();
	WorldVector velocity = new WorldVector(midpoint.x-planeVector.x, midpoint.y-planeVector.y, midpoint.z-planeVector.z);
	
	velocity = setMagnitudeTo(velocity, velocity_multiplier);
	print("new velocity is:\n"+velocity.x+", "+velocity.y+"\n");
	return velocity;
}


private void ActionLanding(){
	List<WorldVector> vertices = get_vertices_from_offset(landing.get_world_object_state().get_position(), landing.get_world_object_state().get_vertices());
	print(landing.get_world_object_state().get_position().toString());
	
	plane.get_world_object_state().set_velocity(calculateHeading(true, landingVelocity, vertices));
	while(!checkDistanceToRunway(plane.get_world_object_state().get_position())){
		print(plane.get_world_object_state().get_position().toString());
		try {
			Thread.sleep(500);
		} catch (InterruptedException e) {
			e.printStackTrace();
		}
	}
	plane.get_world_object_state().set_position(runwayPosition);
	plane.get_world_object_state().set_velocity(calculateHeading(false, landingVelocity, vertices));
	print("Plane Landed\n"+plane.get_world_object_state().get_position().toString());
}

private boolean checkDistanceToRunway(WorldVector myPosition){
	double distance = Math.sqrt(Math.pow(runwayPosition.x-myPosition.x, 2)+Math.pow(runwayPosition.y-myPosition.y, 2));
	if(distance < 41)
		return true;
	else
		return false;
}

private List<WorldVector> get_midpoints(List<WorldVector> vertices) {
	List<WorldVector> midpoints = new ArrayList<WorldVector>();
	for(int worldVectorCounter = 0; worldVectorCounter<3; worldVectorCounter++){
		for(int counter2 = worldVectorCounter+1; counter2<4; counter2++){
			if(vertices.get(worldVectorCounter).x == vertices.get(counter2).x){
				midpoints.add(new WorldVector(vertices.get(worldVectorCounter).x,
						(vertices.get(worldVectorCounter).y+vertices.get(counter2).y)/2,
						vertices.get(worldVectorCounter).z));
			}
		}
	}
	return midpoints;
}


private WorldVector get_midpoint(List<WorldVector> midpoints, boolean closest) {
	
	double distance1 = Double.MAX_VALUE, distance2=0;
	if (!closest)
		distance1 = Double.MIN_VALUE;
	
	WorldVector planeVector = plane.get_world_object_state().get_position();
	WorldVector saved_point = new WorldVector();
	for(int w = 0; w<midpoints.size();w++){
		distance2 = Math.abs(midpoints.get(w).x-planeVector.x);
		if((closest)&&(distance2<distance1)){
			distance1 = distance2;
			saved_point = midpoints.get(w);
		}
		else if((!closest)&&(distance1<distance2)){
			distance1 = distance2;
			saved_point = midpoints.get(w);
		}
	}
	return saved_point;
}



	private WorldVector setMagnitudeTo(WorldVector myVelocity, double new_velocity){
		double myVelocityMagnitude = Math.sqrt(Math.pow(myVelocity.x,2)+Math.pow(myVelocity.y,2)+Math.pow(myVelocity.z,2));
		return myVelocity.times(new_velocity/myVelocityMagnitude);
		
	}
	
	private WorldVector alterMagnitude(WorldVector myVelocity, double velocity_multiplier) {
	
		return myVelocity.times(velocity_multiplier);
	}
	
	public void release_if_necessary() {
		inc_velocity.tryAcquire();
		inc_velocity.release();
	}
	
	public String getName() {
		return name;
	}
	
	public void setLocalController(LocalControllerInterface lc){
		localController = lc;
	}
	public void setGroundController(GroundControllerInterface gc){
		groundController = gc;
	}
	public GroundControllerInterface getGroundController() {
		return groundController;
	}
	public LocalControllerInterface getLocalController() {
		return localController;
	}
	public void msgPrepareForArrival() {
		// TODO Auto-generated method stub
		
	}

	public void msgPrepareForDeparture() {
		// TODO Auto-generated method stub
		
	}
	
	public void setDepartureState() {
		pilotState = PilotState.NEW_DEPARTURE;
	}
	
	public void changeState() {
		stateChanged();
	}
}