def detect2DCollision(lineSegment, polygon):
#(lineSegment:LineSegment) (polygon:Polygon) =
# // Check characteristics of the shapes
# assertGreaterEqualTo "sides" (float polygon.Length) "3 sides" 3.0
# http://stackoverflow.com/questions/12222700/determine-if-line-segment-is-inside-polygon
# first, sort a polygon. Here, polygons are assumed to be sorted already
# // Gather the sides for the polygon
polygonSides = []
for i in range(len(polygon)):
if i == len(polygon) - 1:
endPoint = polygon[0]
else:
endPoint = polygon[i + 1]
polygonSides.append(Spatial.LineSegment(polygon[i], endPoint))
axes = polygonSides
axes.append(lineSegment)
axes = [
Spatial.toUnitVector(
Spatial.toNormalVector(
Spatial.calculateLocationDifference(v.Location1, v.Location2)))
for v in axes
]
# // Check for separation between objects
separationExists = False
for axis in axes:
polyProj = projectPolygonOntoAxis(polygon, axis)
lineSegmentProj = projectSegmentOntoAxis(lineSegment, axis)
#// Do gaps exist between max/min intervals?
if polyProj.Min > lineSegmentProj.Max or lineSegmentProj.Min > polyProj.Max:
separationExists = True
return not separationExists
def stepToWaypoint(coreFunctions, timeStep, arrivalRadius, parameters, weather,
airspace, state, instruction):
#(coreFunctions:SimulationCoreFunctions) (timeStep:float<Hours>) (arrivalRadius:float<NauticalMiles>)
#(parameters:FlightParameters) (weather:WeatherState) (airspace:Airspace) (state:FlightState) (instruction:Instruction) =
(currentX, currentY, currentZ) = state.AircraftPosition
(destinationX, destinationY, destinationZRaw) = instruction.Waypoint
grossWeight = coreFunctions.WeightModel(parameters, state)
destinationZ = coreFunctions.AltitudeLimiter(grossWeight, destinationZRaw)
distanceRemaining = Spatial.calculateDistance1(
Spatial.Location(currentX, currentY),
Spatial.Location(destinationX, destinationY))
(cruiseBurn, groundVector, verticalCruiseFuelDiff, VerticalVelocity, airSpeed, messages) = \
calculateMovement(coreFunctions, parameters, weather, state, instruction)
# assertNonNegative "cruiseBurn" cruiseBurn
groundSpeed = Movement.calculateSpeed(groundVector)
distanceTravelled = groundSpeed * timeStep
reachedDestination = distanceRemaining <= (distanceTravelled +
arrivalRadius)
# print 'distanceRemaing: ' + str(distanceRemaining) + 'distanceTravelled: ' + str(distanceTravelled + arrivalRadius)
(groundSpeedX, groundSpeedY) = groundVector
(groundX, groundY) = (float(groundSpeedX * timeStep),
float(groundSpeedY * timeStep))
elapsedRatio = 0.0
if distanceTravelled > 0.0:
elapsedRatio = min(distanceRemaining,
distanceTravelled) / distanceTravelled
timeElapsed = elapsedRatio * timeStep
climbTimeElapsed = 0.0
if math.fabs(VerticalVelocity) > 0.0:
climbTimeElapsed = \
min(1.0,
((destinationZ - currentZ)/(VerticalVelocity * timeElapsed)) * timeElapsed)
newPosition = Spatial.Position(
currentX + groundX * elapsedRatio, currentY + groundY * elapsedRatio,
currentZ + VerticalVelocity * climbTimeElapsed)
totalFuelBurn = cruiseBurn * timeElapsed + verticalCruiseFuelDiff * climbTimeElapsed
# print 'totalFuelBurn:' + str(totalFuelBurn) + 'curise:' + str(cruiseBurn * timeElapsed) + \
# 'verticalBurn:' + str(verticalCruiseFuelDiff * climbTimeElapsed)
intersectedAirspace = Airspace.aircraftInAirspace(airspace,
state.AircraftPosition,
newPosition)
timeInTurbulence = 0.0
if intersectedAirspace.TurbulentZone:
timeInTurbulence = timeElapsed
# Aircraft's new state
return (reachedDestination,
FlightTypes.FlightState(AircraftPosition=newPosition,
AirSpeed=airSpeed,
GroundSpeed=groundSpeed,
TimeElapsed=timeElapsed,
FuelConsumed=totalFuelBurn,
IntersectedAirspace=intersectedAirspace,
TimeElapsedInTurbulence=timeInTurbulence,
Messages=[]))
def rescale(v, s):
m = calculateSpeed(v)
ratio = 0
if m > 0:
ratio = s / m
try:
r = Spatial.Vector(v.DirectionX * ratio, v.DirectionY * ratio)
except:
r = Spatial.Vector(v.PositionX * ratio, v.PositionY * ratio)
return r
def withinArrivalZone(flightParams, state):
#(flightParams : FlightParameters) (state : FlightState) =
airport = flightParams.DestinationAirport
arrivalAltitude = airport.ArrivalAltitude
arrivalDistance = airport.ArrivalDistance
position = state.AircraftPosition
altitude = Spatial.positionToAltitude(position)
distance = Spatial.calculateDistance(Spatial.positionToLocation(position),
Spatial.positionToLocation(airport.Position))
result = altitude <= arrivalAltitude and distance <= arrivalDistance
return result
def detect3DCollision(lineSegment, altitude, polyhedron):
# (lineSegment:LineSegment) (altitude:float<Feet>) (polyhedron:Polyhedron) =
# // Check bound values
# assertGreaterEqualTo "altitude" (float altitude) "0.0" 0.0
# assertGreaterEqualTo "Zone LowerBound" (float polyhedron.LowerBound) "0.0" 0.0
# assertGreaterThan "Zone UpperBound" (float polyhedron.UpperBound) "LowerBound" (float polyhedron.LowerBound)
segmentMidPoint = Spatial.calculateMidpoint(lineSegment)
#// Check upper/lower bounds and then 2d collision detection
return polyhedron.LowerBound < altitude and altitude < polyhedron.UpperBound \
and Spatial.calculateDistance(segmentMidPoint, polyhedron.Polygon[0]) < 600.0 \
and detect2DCollision(lineSegment, polyhedron.Polygon)
def update_instr(hgt_spd, instruction):
n_ins = len(instruction)
cur_instruction = []
for k in range(n_ins):
position = Spatial.Position(instruction[k][0][0], instruction[k][0][1], ALT_SCALE * hgt_spd[k])
cur_instruction.append( FlightTypes.Instruction(position, SPD_SCALE * hgt_spd[k + n_ins]) )
return cur_instruction
def calculateGroundVelocity(airspeed, groundDirection, wind):
windDirection = Spatial.calculateRadians(wind)
windspeed = Spatial.calculateMagnitude(wind)
aDirection = (windDirection - groundDirection) % ( 2 * math.pi)
precision = 1e-6
# no wind, groundspeed is airspeed
if windspeed <= precision:
return airspeed
elif math.fAbs(aDirection) <= precision: # then // Same direction
return airspeed + windspeed
elif math.fabs(aDirection - math.pi) <= precision: # then // Opposite direction
return airspeed - windspeed
else:
w = math.asin((windspeed * math.sin(aDirection)) / airspeed)
g = math.pi - aDirection - w
return airspeed * math.sin(g) / math.sin(aDirection)
def calculateMovement(coreFunctions, parameters, weather, state, instruction):
# (coreFunctions:SimulationCoreFunctions)
# (parameters:FlightParameters)
# (weather:WeatherState) (state:FlightState) (instruction:Instruction)
time = parameters.ActualGateDepartureTime + state.TimeElapsed*1.0 # <Time/Hours>
currentPosition = state.AircraftPosition
altitude = Spatial.positionToAltitude(currentPosition)
grossWeight = coreFunctions.WeightModel(parameters, state)
(destination, instructedAirSpeed) = instruction
currentLocation = Spatial.positionToLocation(currentPosition)
destinationLocation = Spatial.positionToLocation(destination)
destinationAltitude = coreFunctions.AltitudeLimiter(grossWeight,
Spatial.positionToAltitude(destination))
airSpeed = coreFunctions.AirSpeedLimiter(
parameters.AircraftType.MaximumMachSpeed,
grossWeight, altitude, instructedAirSpeed)
deltaVector = Spatial.calculateLocationDifference(currentLocation, destinationLocation)
distance = Spatial.calculateDistance1(currentLocation, destinationLocation)
if distance <= 0.0 :
zeroGroundSpeed = Spatial.Vector(0.0, 0.0)
return (0.0, zeroGroundSpeed, 0.0, 0.0, 0.0, [])
else:
#windVector = getWindVelocity(weather, time, currentPosition)
windVector = Weather.zeroWindVelocity
groundSpeed = Movement.calculateGroundVelocity(airSpeed,
Spatial.calculateRadians(deltaVector),
windVector)
groundVector = Movement.rescale(deltaVector, groundSpeed)
(cruiseBurn, verticalCruiseFuelDiff, verticalVelocity) = \
coreFunctions.FuelModel(FuelModel.flightFunctions, grossWeight, altitude,
airSpeed,
FlightTypes.getVerticalState(altitude, destinationAltitude))
return (cruiseBurn, groundVector, verticalCruiseFuelDiff, verticalVelocity, airSpeed, [])
def calculateGroundVelocity(airspeed, groundDirection, wind):
windDirection = Spatial.calculateRadians(wind)
windspeed = Spatial.calculateMagnitude(wind)
aDirection = (windDirection - groundDirection) % (2 * math.pi)
precision = 1e-6
# no wind, groundspeed is airspeed
if windspeed <= precision:
return airspeed
elif math.fAbs(aDirection) <= precision: # then // Same direction
return airspeed + windspeed
elif math.fabs(aDirection -
math.pi) <= precision: # then // Opposite direction
return airspeed - windspeed
else:
w = math.asin((windspeed * math.sin(aDirection)) / airspeed)
g = math.pi - aDirection - w
return airspeed * math.sin(g) / math.sin(aDirection)
def stepToWaypoint(coreFunctions, timeStep, arrivalRadius, parameters, weather, airspace,
state, instruction):
#(coreFunctions:SimulationCoreFunctions) (timeStep:float<Hours>) (arrivalRadius:float<NauticalMiles>)
#(parameters:FlightParameters) (weather:WeatherState) (airspace:Airspace) (state:FlightState) (instruction:Instruction) =
(currentX, currentY, currentZ) = state.AircraftPosition
(destinationX, destinationY, destinationZRaw) = instruction.Waypoint
grossWeight = coreFunctions.WeightModel(parameters, state)
destinationZ = coreFunctions.AltitudeLimiter(grossWeight, destinationZRaw)
distanceRemaining = Spatial.calculateDistance1(Spatial.Location(currentX, currentY),
Spatial.Location(destinationX, destinationY))
(cruiseBurn, groundVector, verticalCruiseFuelDiff, VerticalVelocity, airSpeed, messages) = \
calculateMovement(coreFunctions, parameters, weather, state, instruction)
# assertNonNegative "cruiseBurn" cruiseBurn
groundSpeed = Movement.calculateSpeed(groundVector)
distanceTravelled = groundSpeed * timeStep
reachedDestination = distanceRemaining <= (distanceTravelled + arrivalRadius)
# print 'distanceRemaing: ' + str(distanceRemaining) + 'distanceTravelled: ' + str(distanceTravelled + arrivalRadius)
(groundSpeedX, groundSpeedY) = groundVector
(groundX, groundY) = (float (groundSpeedX * timeStep), float (groundSpeedY * timeStep))
elapsedRatio = 0.0
if distanceTravelled > 0.0:
elapsedRatio = min(distanceRemaining, distanceTravelled)/distanceTravelled
timeElapsed = elapsedRatio * timeStep
climbTimeElapsed = 0.0
if math.fabs(VerticalVelocity) > 0.0:
climbTimeElapsed = \
min(1.0,
((destinationZ - currentZ)/(VerticalVelocity * timeElapsed)) * timeElapsed)
newPosition = Spatial.Position(currentX+groundX*elapsedRatio,
currentY+groundY*elapsedRatio,
currentZ + VerticalVelocity*climbTimeElapsed)
totalFuelBurn = cruiseBurn * timeElapsed + verticalCruiseFuelDiff * climbTimeElapsed
# print 'totalFuelBurn:' + str(totalFuelBurn) + 'curise:' + str(cruiseBurn * timeElapsed) + \
# 'verticalBurn:' + str(verticalCruiseFuelDiff * climbTimeElapsed)
intersectedAirspace = Airspace.aircraftInAirspace(airspace, state.AircraftPosition, newPosition)
timeInTurbulence = 0.0
if intersectedAirspace.TurbulentZone:
timeInTurbulence = timeElapsed
# Aircraft's new state
return (reachedDestination,
FlightTypes.FlightState(
AircraftPosition=newPosition,
AirSpeed = airSpeed,
GroundSpeed = groundSpeed,
TimeElapsed = timeElapsed,
FuelConsumed = totalFuelBurn,
IntersectedAirspace = intersectedAirspace,
TimeElapsedInTurbulence = timeInTurbulence,
Messages = []))
def loadAirportsFromFile(file):
f = csv.reader(open(file, 'rb'), delimiter=',')
f.next()
airports = {}
for row in f:
code = str(row[0])
airports[code] = Airport.Airport(
Code=code,
Position=Spatial.Position(float(row[1]), float(row[2]), float(row[3])),
ArrivalDistance = float(row[4]),
ArrivalAltitude = float(row[5]))
return airports
def loadZonesFromFile(file):
f = csv.reader(open(file, 'rb'), delimiter=',')
taxi = {}
f.next()
zones = []
for row in f:
latlongVertStrings = row[3]
lambertVertStrings = row[4]
llArray = []
lamArray = []
for latlong in latlongVertStrings.split(' '):
vs = [float(v) for v in latlong.split(':')]
llArray.append(Spatial.Location(vs[0], vs[1]))
for lambert in lambertVertStrings.split(' '):
vs = [float(v) for v in lambert.split(':')]
lamArray.append(Spatial.Location(vs[0], vs[1]))
zones.append(Zone.Zone(row[0],
llArray,
Spatial.Polyhedron(lamArray, float(row[1]), float(row[2]))
))
return zones
def aircraftInAirspace(airspace, previousPosition, currentPosition):
lineSegment = Spatial.LineSegment(previousPosition, currentPosition)
altitude = currentPosition.Feet
# check whether it cross with the restrictedZone or turbulentZones
crossRestricted = False
crossTurbulent = False
for res in airspace.RestrictedZones:
# print res
if CollisionDetection.detect3DCollision(lineSegment, altitude,
res.Polyhedron):
crossRestricted = True
for turb in airspace.TurbulentZones:
if CollisionDetection.detect3DCollision(lineSegment, altitude,
res.Polyhedron):
crossTurbulent = True
return IntersectAirspace(crossRestricted, crossTurbulent)
def rowToFlightEntry (row):
id = int(row[0])
position = Spatial.Position(float(row[1]), float(row[2]), float(row[3]))
flightEntry = FlightEntry.FlightEntry(id, position,
row[4], row[5],
FlightTypes.Payload(int(row[6]), int(row[7])),
float(row[8]), float(row[9]), float(row[10]),
float(row[11]),
float(row[12]), float(row[13]))
costParameters = CostParameters.CostParameters(
float(row[14]), float(row[15]), float(row[16]), float(row[17]),
float(row[18]), float(row[19]), float(row[20]), float(row[21]),
int(row[22]), float(row[23]), float(row[24]))
return (flightEntry,costParameters)
def calculateMovement(coreFunctions, parameters, weather, state, instruction):
# (coreFunctions:SimulationCoreFunctions)
# (parameters:FlightParameters)
# (weather:WeatherState) (state:FlightState) (instruction:Instruction)
time = parameters.ActualGateDepartureTime + state.TimeElapsed * 1.0 # <Time/Hours>
currentPosition = state.AircraftPosition
altitude = Spatial.positionToAltitude(currentPosition)
grossWeight = coreFunctions.WeightModel(parameters, state)
(destination, instructedAirSpeed) = instruction
currentLocation = Spatial.positionToLocation(currentPosition)
destinationLocation = Spatial.positionToLocation(destination)
destinationAltitude = coreFunctions.AltitudeLimiter(
grossWeight, Spatial.positionToAltitude(destination))
airSpeed = coreFunctions.AirSpeedLimiter(
parameters.AircraftType.MaximumMachSpeed, grossWeight, altitude,
instructedAirSpeed)
deltaVector = Spatial.calculateLocationDifference(currentLocation,
destinationLocation)
distance = Spatial.calculateDistance1(currentLocation, destinationLocation)
if distance <= 0.0:
zeroGroundSpeed = Spatial.Vector(0.0, 0.0)
return (0.0, zeroGroundSpeed, 0.0, 0.0, 0.0, [])
else:
#windVector = getWindVelocity(weather, time, currentPosition)
windVector = Weather.zeroWindVelocity
groundSpeed = Movement.calculateGroundVelocity(
airSpeed, Spatial.calculateRadians(deltaVector), windVector)
groundVector = Movement.rescale(deltaVector, groundSpeed)
(cruiseBurn, verticalCruiseFuelDiff, verticalVelocity) = \
coreFunctions.FuelModel(FuelModel.flightFunctions, grossWeight, altitude,
airSpeed,
FlightTypes.getVerticalState(altitude, destinationAltitude))
return (cruiseBurn, groundVector, verticalCruiseFuelDiff,
verticalVelocity, airSpeed, [])
def calculateSpeed(v):
# v is vector
m = Spatial.calculateMagnitude(v)
return m
def ConvertRawInstruction(projection, raw): #:RawInstruction)
lambertCoordinates = (raw.Latitude, raw.Longitude)
(easting,northing) = LambertConverter.toLambert(projection, lambertCoordinates)
position = Spatial.Position(easting, northing, raw.Feet)
return FlightTypes.Instruction(position, raw.Knots) # instruction
__author__ = 'Adam Stelmack'
__version__ = '2.1.8'
__date__ = 'May 17 2010'
import OSC
import sys
## better practice ##
client = OSC.OSCClient()
client.connect( ('127.0.0.1', 7000) ) # note that the argument is a tupple and not two arguments
msg = OSC.OSCMessage() # we reuse the same variable msg used above overwriting it
#Create an accelerometer object
try:
spatial = Spatial()
except RuntimeError as e:
print("Runtime Exception: %s" % e.details)
print("Exiting....")
exit(1)
#Information Display Function
def DisplayDeviceInfo():
print("|------------|----------------------------------|--------------|------------|")
print("|- Attached -|- Type -|- Serial No. -|- Version -|")
print("|------------|----------------------------------|--------------|------------|")
print("|- %8s -|- %30s -|- %10d -|- %8d -|" % (spatial.isAttached(), spatial.getDeviceName(), spatial.getSerialNum(), spatial.getDeviceVersion()))
print("|------------|----------------------------------|--------------|------------|")
print("Number of Acceleration Axes: %i" % (spatial.getAccelerationAxisCount()))
print("Number of Gyro Axes: %i" % (spatial.getGyroAxisCount()))
print("Number of Compass Axes: %i" % (spatial.getCompassAxisCount()))
from collections import namedtuple import Spatial # zeroWindVelocity = Spatial.Vector(0, 0)
def projectLocationOntoAxis(location, axis):
# (location:Location) (axis:Vector) =
vector = Spatial.locationToVector(location)
return Spatial.calculateDotProduct(vector, axis)
def calculateTimeFuelDistance(model, altitude, weight):
# (model:DescentModel) (altitude:float<Feet>) (weight:float<Pounds>) =
time = calculateTime(model, altitude)
fuel = calculateFuel(model, altitude)
distance = Spatial.calculateDistance(model, altitude, weight)
return (time,fuel,distance)
