pilotagent.java.svn-base

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

		real_conn1 = wos1.get_position().plus(connectionList1.get(x));
		for(int y=0;y<connectionList2.size();y++)
		{
			real_conn2 = wos2.get_position().plus(connectionList2.get(y));
			if(real_conn1.equals(real_conn2))
				return new WorldVector(real_conn1);
		}
	}
	return null;
}

private List<WorldVector> get_vertices_from_offset(WorldVector position, List<WorldVector> offsets) {
	//print("MY VERTICES: "+offsets.size());
	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");
		/*
	plane.get_world_object_state().set_velocity(calculateHeading(true, departingVelocity, vertices));
	while(!checkDistanceToRunway(plane.get_world_object_state().get_position(), runwayPosition, plane.get_world_object_state().get_velocity())){
		//print(plane.get_world_object_state().get_position().toString());
        try {
			inc_velocity.acquire();
		} catch (InterruptedException e) {
			e.//printStackTrace();
		}
		plane.get_world_object_state().set_velocity(alterMagnitude(plane.get_world_object_state().get_velocity(),1.2));
	}
	plane.get_world_object_state().set_position(runwayPosition);
	*/
	
	List<WorldVector> vertices = get_vertices_from_offset(departing.get_world_object_state().get_position(), departing.get_world_object_state().get_vertices());

	//print("Plane taking off\n"+plane.get_world_object_state().get_position().toString());
	WorldVector new_velocity = calculateHeading(false, departingVelocity, vertices);
	rotate_plane(new Double(calculateAngle(new_velocity.x, new_velocity.y)));
	plane.get_world_object_state().set_velocity(new_velocity);
	
	//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));
	//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.1);
		 //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) {
			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);
}
*/

private WorldVector calculateHeading(boolean trueIfClosest, double velocity_multiplier, List<WorldVector> vertices){
	//print("VERTICES: "+vertices.get(0)+", size: "+vertices.size());
	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(){
	////print("vertices: " + landing.get_world_object_state().get_vertices());
	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));
	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(), runwayPosition, plane.get_world_object_state().get_velocity())){
        try {
			inc_velocity.acquire();
			//print(plane.get_world_object_state().get_position().toString());
		} catch (InterruptedException e) {
			e.printStackTrace();
		}
	}
	plane.get_world_object_state().set_position(runwayPosition);
	plane.get_world_object_state().set_velocity(calculateHeading(false, taxiVelocity, 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(), landing.get_world_object_state().get_position(), plane.get_world_object_state().get_velocity())){
        try {
			inc_velocity.acquire();
			if(plane.get_world_object_state().get_velocity().magnitude()>20)
				plane.get_world_object_state().set_velocity(alterMagnitude(plane.get_world_object_state().get_velocity(),.95));
		} catch (InterruptedException e) {
			e.printStackTrace();
		}
	}
	plane.get_world_object_state().set_position(landing.get_world_object_state().get_position());
	plane.get_world_object_state().set_velocity(new WorldVector(0,0,0));
	
	//print("Plane Landed\n"+plane.get_world_object_state().get_position().toString());
}

private boolean checkDistanceToRunway(WorldVector myPosition, WorldVector myRunwayPosition, WorldVector speed){
	double distance = Math.sqrt(Math.pow(myRunwayPosition.x-myPosition.x, 2)+Math.pow(myRunwayPosition.y-myPosition.y, 2));
	double minDistance = 1+ (Math.sqrt(Math.pow(speed.x,2)+Math.pow(speed.y, 2)+Math.pow(speed.z, 2)) / 2);
	if(distance < minDistance)
		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 void rotate_plane(Double future_angle) {	
	
	Double speed = get_angular_velocity(plane.get_world_object_state().angularPosition.z, future_angle);
	
	plane.get_world_object_state().set_angular_velocity(new WorldVector(0, 0, speed));
	
	boolean pos_rot = get_pos_rot(future_angle);
	
	while (!rotated_enough(future_angle, pos_rot)) {
		
		try {
			inc_velocity.acquire();
		} catch (InterruptedException e) { e.printStackTrace();	}
		
	}
	
	plane.get_world_object_state().angularVelocity = new WorldVector();
	plane.get_world_object_state().angularPosition = new WorldVector(0, 0, future_angle);
}

public boolean rotated_enough(Double angle, boolean pos_rot) {
	//System.out.println(pos_rot + " " + get_pos_rot(angle) + " " + angle + " " + plane.get_world_object_state().angularPosition.z);
	
	if (get_pos_rot(angle) == pos_rot)
		return false;
	else {
		return true;
	}
	
	//return true;
}

public boolean get_pos_rot(Double angle) {
	return (get_angular_velocity(angle, plane.get_world_object_state().angularPosition.z) > 0);
}

public Double get_angular_velocity(Double a1, Double a2) {
	
	Double b1 = normalize_angle(a1);
	Double b2 = normalize_angle(a2);
	
	Double speed;
	
	if (((b1-b2) >0)&&((b1-b2) < Math.PI))
		speed = -PilotAgent.turn_velocity;
	else if ((b1-b2) < -Math.PI)
		speed = -PilotAgent.turn_velocity;
	else
		speed = PilotAgent.turn_velocity;
		
		

	//System.out.println("angles: " + Math.toDegrees(a1) + " " + Math.toDegrees(a2));
	//System.out.println("normalized angles: " + Math.toDegrees(b1) + " " + Math.toDegrees(b2));
	//System.out.println("speed: " + Math.toDegrees(speed));
			
	return speed;
	
}

public Double normalize_angle(Double angle) {
	Double new_angle = angle;
	
	while (Math.abs(new_angle) >= 2*Math.PI)
		new_angle = new_angle - (2*Math.PI);
	
	
	if (new_angle < 0)
		new_angle = 2*Math.PI - new_angle;
	
	return new_angle;
}

public String getName() {
	return name;
}

public GroundControllerInterface getGroundController() {
	return groundController;
}
public LocalControllerInterface getLocalController() {
	return localController;
}
public void setGroundController(GroundControllerInterface gc) {
	groundController = gc;
}
public void setLocalController(LocalControllerInterface lc) {
	localController = lc;
}
public void changeState() {
	stateChanged();
}

}