예제 #1
0
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
예제 #2
0
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=[]))
예제 #3
0
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
예제 #4
0
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
예제 #5
0
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)
예제 #6
0
파일: Main.py 프로젝트: Cesarbre/ge_contest
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    
예제 #7
0
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)
예제 #8
0
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, [])
예제 #9
0
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)
예제 #10
0
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 = []))
예제 #11
0
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
예제 #12
0
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
예제 #13
0
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)
예제 #14
0
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)
예제 #15
0
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, [])
예제 #16
0
def calculateSpeed(v):
    # v is vector
    m = Spatial.calculateMagnitude(v)
    return m
예제 #17
0
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()))
예제 #19
0
from collections import namedtuple
import Spatial
#

zeroWindVelocity = Spatial.Vector(0, 0)
예제 #20
0
def projectLocationOntoAxis(location, axis):
    # (location:Location) (axis:Vector) =
    vector = Spatial.locationToVector(location)
    return Spatial.calculateDotProduct(vector, axis)
예제 #21
0
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)