Aircraft.pd

//*******************************************************
//* File:    C:/Users/User/Desktop/Third Year Project/CollisionAvoidanceSystem/Aircraft.pd
//* Author:  Varuna
//* Created: 11:49:59 on Thursday January 8th 2015 UTC
//*******************************************************

class Aircraft ^=
abstract

	const angleScale ^= 0.52;
	const verticalBreakPoint ^= 8800.00; //meters
	const cosOfDescentAngle ^= 0.99; //cos 3.0 degrees
	
	var position : Vector;
	var velocity : Vector; // km or m per sec
	var boundaryRadius : real; // km or m as above
	var status : AircraftStatus;
	var identification : string;
	var conflictStatus : ConflictStatus;
	var model : AircraftModel;
	var targetHeight : real;
	
	invariant position.type = VectorType Position;
	invariant velocity.type = VectorType Velocity;
	invariant ~identification.empty;

confined
	
	function futureConflict(other : Aircraft) : bool
		^= breaksMinimumVerticalSeparation(other) & (getMinimum2DDistanceBetweenPaths(other) <= getBiggerRadius(other));
		     
	function getMinimum2DDistanceBetweenPaths(other : Aircraft) : real
		^= (let tForMinD ^= getTimeToMinimumDistanceBetweenPaths(other);
			([tForMinD < 0.0] : 100000.00, //any large number
			 [] : positionAfterFlying(tForMinD).getTwoDimensionalDistanceFrom(other.positionAfterFlying(tForMinD))));
			 
	function getTimeToMinimumDistanceBetweenPaths(other : Aircraft) : real
		^= (let magSquare ^= getSquareOfMagnitudeOfPostionOfOtherWRTThis(other);
		   magSquare.getXForMinimumValue);
	
	function getSquareOfMagnitudeOfPostionOfOtherWRTThis(other : Aircraft) : Equation
		^= (let q1 ^= VectorWithVariables{position.x, position.y, position.z, velocity.x, velocity.y, velocity.z};
		   let q2 ^= VectorWithVariables{other.position.x, other.position.y, other.position.z, other.velocity.x, other.velocity.y, other.velocity.z};
		   let q21 ^= q2 - q1;
		   q21.squareOf2DMagnitude);
		   
	function getBiggerRadius(other : Aircraft) : real
		^= ([boundaryRadius > other.boundaryRadius] : boundaryRadius,
			[] : other.boundaryRadius);
	
	function isInConflictWith(other : Aircraft) : bool
		^= breaksMinimumVerticalSeparation(other) & 
		   (position.getTwoDimensionalDistanceFrom(other.position) <= getBiggerRadius(other));
			
	function timeToCollision(other : Aircraft) : real
		pre ~isInConflictWith(other) & other.velocity ~= velocity
		^= (let velocityDifference ^= other.velocity - velocity;
			((position - other.position).dotProduct(velocityDifference)/((velocityDifference.magnitude)^2)))
		assert result >= 0.00;
	
	function getCosineOfAngleForClimb : real
		^=([model = AircraftModel Boeing777200 | model = AircraftModel Boeing777300] : 0.97, //cos 14.0,
		   [model = AircraftModel Boeing737300 | model = AircraftModel Boeing737400 | model = AircraftModel Boeing737500] : 0.96, //cos15.0
		   [model = AircraftModel Boeing717] : 0.95, //cos 17.0
		   [model = AircraftModel Boeing737600 | model = AircraftModel Boeing737900] : 0.96, //cos 15.5
		   [model = AircraftModel BoeingMD80] : 0.95, //cos 17.0
		   [model = AircraftModel Boeing747400] : 0.96, //cos15.0
		   [model = AircraftModel BoeingMD90] : 0.95, //cos 17.0
		   [model = AircraftModel Boeing757200 | model = AircraftModel Boeing767400] : 0.96, //cos15.0
		   [model = AircraftModel BoeingMD11] : 0.94); //cos 20.0
			
	function getVelocityForAscentOrDescent(isAscent : bool) : Vector
		^= (let cosOfAngle ^= ([isAscent] : getCosineOfAngleForClimb,
							   [] : cosOfDescentAngle) * angleScale;
			let vx2plusvy2 ^= (((velocity.x) ^ 2) + ((velocity.y) ^ 2));
			let vz ^= (((vx2plusvy2/((cosOfAngle)*(vx2plusvy2 ^ (0.5)))) ^ 2) - (vx2plusvy2)) ^ (0.5);//calculated using dot product
			let elevationMultiplier ^= ([isAscent] : 1.0,
										[] : -1.0);
			Vector{velocity.x, velocity.y, vz * elevationMultiplier, VectorType Velocity});
			
	function hasCrossedTargetHeight(newPosition : Vector) : bool
		^=  ((status = AircraftStatus Ascending) & position.z < targetHeight & targetHeight <= newPosition.z) 
										| ((status = AircraftStatus Descending) & position.z > targetHeight & targetHeight >= newPosition.z);
	
	function getPositionAfterStraightFlight(time : real, velocityForFlight : Vector, startPosition : Vector) : Vector
		^= startPosition.plus((velocityForFlight * time), VectorType Position);
		
	function getTimeToTargetHeight : real
		^= (targetHeight - position.z) / velocity.z;
			
	build{!position : Vector, !velocity : Vector, !targetHeight : real, !boundaryRadius : real, !status : AircraftStatus, !identification : string, !model : AircraftModel}
		pre  position.type = VectorType Position & velocity.type = VectorType Velocity
		post conflictStatus != ConflictStatus NoConflict;
		
	build{!position : Vector, !velocity : Vector, !targetHeight : real, !boundaryRadius : real, !status : AircraftStatus, !identification : string, !conflictStatus : ConflictStatus, !model : AircraftModel}
		pre  position.type = VectorType Position & velocity.type = VectorType Velocity;
			
interface	
	
	function conflictStatus;
	
	function position;
	
	function velocity;
	
	function boundaryRadius;
	
	function identification;
	
	function speed : real
		^= velocity.magnitude;
	
	function getMinimumVerticalSeparation(other : Aircraft) : real
		^= ([position.z <= verticalBreakPoint & other.position.z <= verticalBreakPoint] : 300.00, //meters
			[] : 600.00); //meters
			
	function getHeightDifference(other : Aircraft) : real
		^= ([other.position.z > position.z] : other.position.z - position.z,
									 [] : position.z - other.position.z);
		
	function breaksMinimumVerticalSeparation(other : Aircraft) : bool
		^= getHeightDifference(other) < getMinimumVerticalSeparation(other); //meters
	
	function getConflictStatus(other : Aircraft) : ConflictStatus
		^= ([~breaksMinimumVerticalSeparation(other)] : ConflictStatus NoConflict,
			[isInConflictWith(other)] : ConflictStatus Conflicted,
			[futureConflict(other)] : ConflictStatus PotentialFutureConflict,
			[] : ConflictStatus NoConflict);
		 
	function positionAfterFlying(time : real) : Vector
		^= position.plus((velocity * time), VectorType Position);
	
	function getCraftAfterFlying(time : real) : Aircraft
		^= (let posAfterStraightFlight ^= positionAfterFlying(time);
			([hasCrossedTargetHeight(posAfterStraightFlight)] : (let timeToTarget ^= getTimeToTargetHeight;
										 let posAtTarget ^= positionAfterFlying(timeToTarget);
										 let newVelocity ^= Vector{velocity.x, velocity.y, VectorType Velocity};
										 let newPos ^= getPositionAfterStraightFlight(time - timeToTarget, newVelocity, posAtTarget);
										 Aircraft {newPos, newVelocity, 0.0, boundaryRadius, AircraftStatus FlyingAtLevel, identification, model}),
			[] : Aircraft {posAfterStraightFlight, velocity, targetHeight, boundaryRadius, status, identification, model}));
	
	
	function timeToConflict(other : Aircraft) : real
		^= ([~futureConflict(other)] : 100000.00, //any large number
			[] : (let magSquare ^= getSquareOfMagnitudeOfPostionOfOtherWRTThis(other);
				  let eqn ^= magSquare - Equation{0.0, 0.0, (getBiggerRadius(other) ^ 2.0)};
				  eqn.solve));
				  
	redefine function toString : string
		^= "\n Id:" ++ identification ++ "\n Position: " ++ position.toString ++ "\n Velocity: " ++ velocity.toString ++ "\n Conflict Status: " ++ conflictStatus.toString ++ "\n" ++ boundaryRadius.toString;
	
	function getCraftWithConflictStatus(cs : ConflictStatus) : Aircraft
		^= Aircraft{position, velocity, targetHeight, boundaryRadius, status, identification, cs, model};
	
	function getCraftTargettingHeight(heightToTarget : real) : Aircraft
		^= ([heightToTarget = position.z] : Aircraft{position, velocity, targetHeight, boundaryRadius, status, identification, model},
			[heightToTarget > position.z] : Aircraft{position, getVelocityForAscentOrDescent(true), heightToTarget, boundaryRadius, AircraftStatus Ascending, identification, model},
			[heightToTarget < position.z] : Aircraft{position, getVelocityForAscentOrDescent(false), heightToTarget, boundaryRadius, AircraftStatus Descending, identification, model});
	
	build{!position : Vector, !velocity : Vector, !boundaryRadius : real, !identification : string, !model : AircraftModel}
		pre  position.type = VectorType Position & velocity.type = VectorType Velocity & ~identification.empty
		post conflictStatus != ConflictStatus NoConflict,
			status != AircraftStatus FlyingAtLevel,
			targetHeight != 0.0;
	
end;