Changeset 3187


Ignore:
Timestamp:
Jul 31, 2018 10:32:34 AM (6 years ago)
Author:
sward
Message:

Reworked agent pathfinding and modified output

Location:
palm/trunk/SOURCE
Files:
2 edited

Legend:

Unmodified
Added
Removed
  • palm/trunk/SOURCE/multi_agent_system_mod.f90

    r3165 r3187  
    2525! -----------------
    2626! $Id$
     27! Reworked agent pathfinding to avoid collisions with walls
     28!
     29! 3165 2018-07-24 13:12:42Z sward
    2730! Added agent ID output
    2831!
     
    9295    INTEGER(iwp) ::  number_of_agent_groups = 1  !< namelist parameter (see documentation)
    9396    INTEGER(iwp) ::  sort_count_mas = 0          !< counter for sorting agents
     97    INTEGER(iwp) ::  agt_path_size = 15          !< size of agent path array
    9498    INTEGER(iwp) ::  step_dealloc_mas = 100      !< namelist parameter (see documentation)
    9599    INTEGER(iwp) ::  total_number_of_agents      !< total number of agents in the whole model domain
     
    120124    REAL(wp) ::  alloc_factor_mas = 20.0_wp          !< namelist parameter (see documentation)
    121125    REAL(wp) ::  coll_t_0 = 3.                       !< namelist parameter (see documentation)
    122     REAL(wp) ::  corner_gate_start = 0.7_wp          !< namelist parameter (see documentation)
     126    REAL(wp) ::  corner_gate_start = 0.5_wp          !< namelist parameter (see documentation)
    123127    REAL(wp) ::  corner_gate_width = 1.0_wp          !< namelist parameter (see documentation)   
    124128    REAL(wp) ::  d_sigma_rep_agent                   !< inverse of sigma_rep_agent
     
    128132    REAL(wp) ::  des_sp_sig = .2_wp                  !< namelist parameter (see documentation)
    129133    REAL(wp) ::  dist_target_reached = 2.0_wp        !< distance at which target counts as reached
    130     REAL(wp) ::  dist_to_int_target = .75_wp         !< namelist parameter (see documentation)
    131     REAL(wp) ::  d_sqrt_2 = 1.0_wp/SQRT(2.)          !< 1/sqrt(2)
    132     REAL(wp) ::  dt_agent = 0.016_wp                 !< namelist parameter (see documentation)
     134    REAL(wp) ::  dist_to_int_target = .25_wp         !< namelist parameter (see documentation)
     135    REAL(wp) ::  dt_agent = 0.02_wp                  !< namelist parameter (see documentation)
    133136    REAL(wp) ::  dt_arel = 9999999.9_wp              !< namelist parameter (see documentation)
    134137    REAL(wp) ::  end_time_arel = 9999999.9_wp        !< namelist parameter (see documentation)
    135138    REAL(wp) ::  force_x                             !< dummy value for force on current agent in x-direction
    136139    REAL(wp) ::  force_y                             !< dummy value for force on current agent in y-direction
    137     REAL(wp) ::  max_dist_from_path = 1.0_wp         !< distance from current path at which a new path is calculated
     140    REAL(wp) ::  max_dist_from_path = 0.25_wp        !< distance from current path at which a new path is calculated
    138141    REAL(wp) ::  multi_agent_system_start = 0.0_wp   !< namelist parameter (see documentation)
    139     REAL(wp) ::  radius_agent = .2_wp                !< namelist parameter (see documentation)
     142    REAL(wp) ::  radius_agent = .25_wp               !< namelist parameter (see documentation)
    140143    REAL(wp) ::  repuls_agent = 1.5_wp               !< namelist parameter (see documentation)
    141144    REAL(wp) ::  repuls_wall = 7.0_wp                !< namelist parameter (see documentation)
     
    143146    REAL(wp) ::  scan_radius_wall = 2.0_wp           !< namelist parameter (see documentation)
    144147    REAL(wp) ::  sigma_rep_agent = 0.3_wp            !< namelist parameter (see documentation)
    145     REAL(wp) ::  sigma_rep_wall = 0.2_wp             !< namelist parameter (see documentation)
     148    REAL(wp) ::  sigma_rep_wall = 0.1_wp             !< namelist parameter (see documentation)
    146149    REAL(wp) ::  tau_accel_agent = 0.5_wp            !< namelist parameter (see documentation)
    147150    REAL(wp) ::  time_arel = 0.0_wp                  !< time for agent release
    148151    REAL(wp) ::  time_write_agent_data = 0.0_wp      !< write agent data at current time on file
    149152    REAL(wp) ::  v_max_agent = 1.3_wp                !< namelist parameter (see documentation)
    150    
     153
     154    REAL(wp), DIMENSION(:), ALLOCATABLE ::  dummy_path_x  !<  dummy path (x-coordinate)
     155    REAL(wp), DIMENSION(:), ALLOCATABLE ::  dummy_path_y  !<  dummy path (y-coordinate)
    151156
    152157    REAL(wp), DIMENSION(max_number_of_agent_groups) ::  adx = 9999999.9_wp  !< namelist parameter (see documentation)
     
    161166!-- Type for the definition of an agent
    162167    TYPE agent_type
    163         INTEGER(iwp) ::  block_nr            !< number for sorting
    164         INTEGER(iwp) ::  group               !< number of agent group
    165         INTEGER(idp) ::  id                  !< particle ID (64 bit integer)
    166         INTEGER(iwp) ::  path_counter        !< current target along path (path_x/y)
    167         LOGICAL      ::  agent_mask          !< if this parameter is set to false the agent will be deleted
    168         REAL(wp)     ::  age                 !< age of agent
    169         REAL(wp)     ::  age_m               !< age of agent
    170         REAL(wp)     ::  dt_sum              !< sum of agents subtimesteps
    171         REAL(wp)     ::  clo                 !< clothing index
    172         REAL(wp)     ::  energy_storage      !< energy stored by agent
    173         REAL(wp)     ::  force_x             !< force term x-direction
    174         REAL(wp)     ::  force_y             !< force term y-direction
    175         REAL(wp)     ::  origin_x            !< origin x-position of agent
    176         REAL(wp)     ::  origin_y            !< origin y-position of agent
    177         REAL(wp)     ::  pm10                !< PM10 concentration at agent position
    178         REAL(wp)     ::  pm25                !< PM25 concentration at agent position
    179         REAL(wp)     ::  speed_abs           !< absolute value of agent speed
    180         REAL(wp)     ::  speed_e_x           !< normalized speed of agent in x
    181         REAL(wp)     ::  speed_e_y           !< normalized speed of agent in y
    182         REAL(wp)     ::  speed_des           !< agent's desired speed
    183         REAL(wp)     ::  speed_x             !< speed of agent in x
    184         REAL(wp)     ::  speed_y             !< speed of agent in y
    185         REAL(wp)     ::  thermal_index       !< the dynamic thermal index
    186         REAL(wp)     ::  windspeed           !< absolute value of windspeed at agent position
    187         REAL(wp)     ::  x                   !< x-position
    188         REAL(wp)     ::  y                   !< y-position
    189         REAL(wp)     ::  t                   !< temperature
    190         REAL(wp)     ::  t_x                 !< x-position
    191         REAL(wp)     ::  t_y                 !< y-position
     168        INTEGER(iwp) ::  block_nr        !< number for sorting
     169        INTEGER(iwp) ::  group           !< number of agent group
     170        INTEGER(idp) ::  id              !< particle ID (64 bit integer)
     171        INTEGER(iwp) ::  path_counter    !< current target along path (path_x/y)
     172        LOGICAL      ::  agent_mask      !< if this parameter is set to false the agent will be deleted
     173        REAL(wp)     ::  age             !< age of agent
     174        REAL(wp)     ::  age_m           !< age of agent
     175        REAL(wp)     ::  dt_sum          !< sum of agents subtimesteps
     176        REAL(wp)     ::  clo             !< clothing index
     177        REAL(wp)     ::  energy_storage  !< energy stored by agent
     178        REAL(wp)     ::  force_x         !< force term x-direction
     179        REAL(wp)     ::  force_y         !< force term y-direction
     180        REAL(wp)     ::  origin_x        !< origin x-position of agent
     181        REAL(wp)     ::  origin_y        !< origin y-position of agent
     182        REAL(wp)     ::  pm10            !< PM10 concentration at agent position
     183        REAL(wp)     ::  pm25            !< PM25 concentration at agent position
     184        REAL(wp)     ::  speed_abs       !< absolute value of agent speed
     185        REAL(wp)     ::  speed_e_x       !< normalized speed of agent in x
     186        REAL(wp)     ::  speed_e_y       !< normalized speed of agent in y
     187        REAL(wp)     ::  speed_des       !< agent's desired speed
     188        REAL(wp)     ::  speed_x         !< speed of agent in x
     189        REAL(wp)     ::  speed_y         !< speed of agent in y
     190        REAL(wp)     ::  thermal_index   !< the dynamic thermal index
     191        REAL(wp)     ::  windspeed       !< absolute value of windspeed at agent position
     192        REAL(wp)     ::  x               !< x-position
     193        REAL(wp)     ::  y               !< y-position
     194        REAL(wp)     ::  t               !< temperature
     195        REAL(wp)     ::  t_x             !< x-position
     196        REAL(wp)     ::  t_y             !< y-position
    192197        REAL(wp), DIMENSION(0:15) ::  path_x !< agent path to target (x)
    193198        REAL(wp), DIMENSION(0:15) ::  path_y !< agent path to target (y)
     
    514519       INTEGER(iwp) ::  n  !< loop variable over all agents in a grid box
    515520       INTEGER(iwp) ::  pc !< agent path counter
     521       INTEGER(iwp) ::  nobo !< agent path counter
    516522
    517523       REAL(wp) ::  abs_dir         !< length of direction vector (for normalization)
     
    535541!--       If no path was calculated for agent yet, do it
    536542          IF ( pc >= 999 ) THEN
    537 
    538              agents(n)%path_x(0) = agents(n)%origin_x
    539              agents(n)%path_y(0) = agents(n)%origin_y
    540543             CALL mas_nav_find_path(n)
    541544             pc = agents(n)%path_counter
     
    577580!--          of her deviation from her current path
    578581             IF ( path_flag ) THEN
    579                 agents(n)%path_x(0) = agents(n)%path_x(pc-1)
    580                 agents(n)%path_y(0) = agents(n)%path_y(pc-1)
    581582                CALL mas_nav_find_path(n)
    582583                pc = agents(n)%path_counter
     
    10061007          DO jl = nysg, nyng
    10071008!
     1009!--          Exclude cyclic topography boundary
     1010             IF ( il < 0 .OR. il > nx .OR. jl < 0 .OR. jl > ny ) CYCLE
     1011!
    10081012!--          North edge
    10091013             IF ( jl < nyng ) THEN
     
    30653069!--       Allocate agent arrays and set attributes of the initial set of
    30663070!--       agents, which can be also periodically released at later times.
    3067           ALLOCATE( agt_count  (nysg:nyng,nxlg:nxrg),                            &
     3071          ALLOCATE( agt_count  (nysg:nyng,nxlg:nxrg),                          &
    30683072                    grid_agents(nysg:nyng,nxlg:nxrg) )
     3073!
     3074!--       Allocate dummy arrays for pathfinding
     3075          ALLOCATE( dummy_path_x(0:agt_path_size),                 &
     3076                    dummy_path_y(0:agt_path_size) )
    30693077
    30703078          number_of_agents = 0
     
    30843092          zero_agent%group         = 0
    30853093          zero_agent%id            = 0_idp
    3086           zero_agent%path_counter  = SIZE(zero_agent%path_x)
     3094          zero_agent%path_counter  = agt_path_size
    30873095          zero_agent%age           = 0.0_wp
    30883096          zero_agent%age_m         = 0.0_wp
     
    31263134    END SUBROUTINE mas_init
    31273135
     3136!------------------------------------------------------------------------------!
     3137! Description:
     3138! ------------
     3139!> Finds the shortest path from a start position to a target position using the
     3140!> A*-algorithm
     3141!------------------------------------------------------------------------------!
     3142    SUBROUTINE mas_nav_a_star( start_x, start_y, target_x, target_y, nsteps )
     3143
     3144       IMPLICIT NONE
     3145
     3146       LOGICAL ::  target_reached !< flag
     3147
     3148       INTEGER(iwp) ::  cur_node     !< current node of binary heap
     3149       INTEGER(iwp) ::  i_ag         !< agent index
     3150       INTEGER(iwp) ::  il           !< counter (x)
     3151       INTEGER(iwp) ::  jl           !< counter (y)
     3152       INTEGER(iwp) ::  neigh_node   !< neighbor node
     3153       INTEGER(iwp) ::  node_counter !< binary heap node counter
     3154       INTEGER(iwp) ::  path_ag      !< index of agent path
     3155       INTEGER(iwp) ::  som          !< size of mesh
     3156       INTEGER(iwp) ::  rn_side      !< index of agent path
     3157       INTEGER(iwp) ::  steps        !< steps along the path
     3158       INTEGER(iwp) ::  nsteps        !< number of steps
     3159
     3160       REAL(wp) ::  start_x      !< x-coordinate agent
     3161       REAL(wp) ::  start_y      !< y-coordinate agent
     3162       REAL(wp) ::  new_cost     !< updated cost to reach node
     3163       REAL(wp) ::  new_priority !< priority of node to be added to queue
     3164       REAL(wp) ::  rand_nr      !< x-coordinate target
     3165       REAL(wp) ::  rn_gate      !< random number for corner gate
     3166       REAL(wp) ::  target_x     !< x-coordinate target
     3167       REAL(wp) ::  target_y     !< y-coordinate target
     3168!
     3169!--    Coordinate Type
     3170       TYPE coord
     3171          REAL(wp) ::  x   !< x-coordinate
     3172          REAL(wp) ::  x_s !< x-coordinate (shifted)
     3173          REAL(wp) ::  y   !< y-coordinate
     3174          REAL(wp) ::  y_s !< y-coordinate (shifted)
     3175       END TYPE coord
     3176
     3177       TYPE(coord), DIMENSION(:), ALLOCATABLE, TARGET ::  path     !< path array
     3178       TYPE(coord), DIMENSION(:), ALLOCATABLE, TARGET ::  tmp_path !< temporary path for resizing
     3179
     3180       CALL cpu_log( log_point_s(20), 'mas_nav_find_path', 'start' )
     3181       node_counter = 0
     3182
     3183!
     3184!--    Create temporary navigation mesh including agent and target positions
     3185       CALL mas_nav_create_tmp_mesh( start_x, start_y, target_x, target_y, som )
     3186       tmp_mesh(som)%cost_so_far = 0.0_wp
     3187!
     3188!--    Initialize priority queue
     3189       heap_count = 0_iwp
     3190       ALLOCATE(queue(0:100))
     3191       target_reached = .FALSE.
     3192!
     3193!--    Add starting point (agent position) to frontier (the frontier consists
     3194!--    of all the nodes that are to be visited. The node with the smallest
     3195!--    priority will be visited first. The priority consists of the distance
     3196!--    from the start node to this node plus a minimal guess (direct distance)
     3197!--    from this node to the goal). For the starting node, the priority is set
     3198!--    to 0, as it's the only node thus far
     3199       CALL mas_heap_insert_item(som,0.0_wp)
     3200       cur_node = som
     3201       DO WHILE ( heap_count > 0 )
     3202!
     3203!--       Step one: Pick lowest priority item from queue
     3204          node_counter = node_counter + 1
     3205          CALL mas_heap_extract_item(cur_node)
     3206!
     3207!--       Node 0 is the goal node
     3208          IF ( cur_node == 0 ) THEN
     3209             EXIT
     3210          ENDIF
     3211!
     3212!--       Loop over all of cur_node's neighbors
     3213          DO il = 1, tmp_mesh(cur_node)%noc
     3214             neigh_node = tmp_mesh(cur_node)%connected_vertices(il)
     3215!
     3216!--          Check, if the way from the start node to this neigh_node via
     3217!--          cur_node is shorter than the previously found shortest path to it.
     3218!--          If so, replace said cost and add neigh_node to the frontier.
     3219!--          cost_so_far is initialized as 1.d12 so that all found distances
     3220!--          should be smaller.
     3221             new_cost   = tmp_mesh(cur_node)%cost_so_far                       &
     3222                         + tmp_mesh(cur_node)%distance_to_vertex(il)
     3223             IF ( new_cost < tmp_mesh(neigh_node)%cost_so_far ) THEN
     3224                tmp_mesh(neigh_node)%cost_so_far = new_cost
     3225                tmp_mesh(neigh_node)%origin_id   = cur_node
     3226!
     3227!--             Priority in the queue is cost_so_far + heuristic to goal
     3228                new_priority = new_cost                                        &
     3229                              + heuristic(tmp_mesh(neigh_node)%x,              &
     3230                                tmp_mesh(neigh_node)%y, tmp_mesh(0)%x,         &
     3231                                tmp_mesh(0)%y)
     3232                CALL mas_heap_insert_item(neigh_node,new_priority)
     3233             ENDIF
     3234          ENDDO
     3235       ENDDO
     3236!
     3237!--    Add nodes to a path array. To do this, we must backtrack from the target
     3238!--    node to its origin to its origin and so on until an node is reached that
     3239!--    has no origin (%origin_id == -1). This is the starting node.
     3240       DEALLOCATE(queue)
     3241       cur_node = 0
     3242       steps = 0
     3243       ALLOCATE(path(1:100))
     3244       DO WHILE ( cur_node /= -1 )
     3245          steps = steps + 1
     3246!
     3247!--       Resize path array if necessary
     3248          IF ( steps > SIZE(path) ) THEN
     3249             ALLOCATE(tmp_path(1:steps-1))
     3250             tmp_path(1:steps-1) = path(1:steps-1)
     3251             DEALLOCATE(path)
     3252             ALLOCATE(path(1:2*(steps-1)))
     3253             path(1:steps-1) = tmp_path(1:steps-1)
     3254             DEALLOCATE(tmp_path)
     3255          ENDIF
     3256          path(steps)%x = tmp_mesh(cur_node)%x
     3257          path(steps)%y = tmp_mesh(cur_node)%y
     3258          path(steps)%x_s = tmp_mesh(cur_node)%x_s
     3259          path(steps)%y_s = tmp_mesh(cur_node)%y_s
     3260          cur_node = tmp_mesh(cur_node)%origin_id
     3261       ENDDO
     3262!
     3263!--    Add calculated intermittent targets to the path until either the
     3264!--    target or the maximum number of intermittent targets is reached.
     3265!--    Ignore starting point (reduce index by one), it is agent position.
     3266       dummy_path_x = -1
     3267       dummy_path_y = -1
     3268       path_ag = 1
     3269       steps = steps - 1
     3270       nsteps = 0
     3271       DO WHILE( steps > 0 .AND. path_ag <= agt_path_size )
     3272!
     3273!--       Each target point is randomly chosen along a line target along the
     3274!--       bisector of the building corner that starts at corner_gate_start
     3275!--       and has a width of corner_gate_width. This is to avoid clustering
     3276!--       when opposing agent groups try to reach the same corner target.
     3277          rn_gate = random_function(iran_agent) * corner_gate_width            &
     3278                                                + corner_gate_start
     3279          dummy_path_x(path_ag) = path(steps)%x + rn_gate                      &
     3280                         * (path(steps)%x_s - path(steps)%x)
     3281          dummy_path_y(path_ag) = path(steps)%y + rn_gate                      &
     3282                         * (path(steps)%y_s - path(steps)%y)
     3283          steps = steps - 1
     3284          path_ag = path_ag + 1
     3285          nsteps = nsteps + 1
     3286       ENDDO
     3287!
     3288!--    Set current intermittent target of this agent
     3289       DEALLOCATE(tmp_mesh, path)
     3290       CALL cpu_log( log_point_s(20), 'mas_nav_find_path', 'stop' )
     3291
     3292    END SUBROUTINE mas_nav_a_star
     3293
    31283294!------------------------------------------------------------------------------!
    31293295! Description:
     
    33243490
    33253491    END SUBROUTINE mas_nav_create_tmp_mesh
     3492   
    33263493
    33273494!------------------------------------------------------------------------------!
    33283495! Description:
    33293496! ------------
    3330 !> Finds the shortest path from agent position to target using A*-algorithm
    3331 !------------------------------------------------------------------------------!
    3332     SUBROUTINE mas_nav_find_path( i_ag )
     3497!> Finds the shortest path from an agents' position to her target. As the
     3498!> actual pathfinding algorithm uses the obstacle corners and then shifts them
     3499!> outward after pathfinding, cases can uccur in which the connection between
     3500!> these intermittent targets then intersect with obstacles. To remedy this
     3501!> the pathfinding algorithm is then run on every two subsequent intermittent
     3502!> targets iteratively and new intermittent targets may be added to the path
     3503!> this way.
     3504!------------------------------------------------------------------------------!
     3505    SUBROUTINE mas_nav_find_path( nl )
    33333506
    33343507       IMPLICIT NONE
    33353508
    3336        LOGICAL ::  target_reached !< flag
    3337 
    3338        INTEGER(iwp) ::  cur_node     !< current node of binary heap
    3339        INTEGER(iwp) ::  i_ag         !< agent index
    3340        INTEGER(iwp) ::  il           !< counter (x)
    3341        INTEGER(iwp) ::  jl           !< counter (y)
    3342        INTEGER(iwp) ::  neigh_node   !< neighbor node
    3343        INTEGER(iwp) ::  node_counter !< binary heap node counter
    3344        INTEGER(iwp) ::  path_ag      !< index of agent path
    3345        INTEGER(iwp) ::  som          !< size of mesh
    3346        INTEGER(iwp) ::  rn_side      !< index of agent path
    3347        INTEGER(iwp) ::  steps        !< steps along the path
    3348 
    3349        REAL(wp) ::  a_x          !< x-coordinate agent
    3350        REAL(wp) ::  a_y          !< y-coordinate agent
    3351        REAL(wp) ::  new_cost     !< updated cost to reach node
    3352        REAL(wp) ::  new_priority !< priority of node to be added to queue
    3353        REAL(wp) ::  rand_nr      !< x-coordinate target
    3354        REAL(wp) ::  rn_gate      !< random number for corner gate
    3355        REAL(wp) ::  t_x          !< x-coordinate target
    3356        REAL(wp) ::  t_y          !< y-coordinate target
    3357 !
    3358 !--    Coordinate Type
    3359        TYPE coord
    3360           REAL(wp) ::  x   !< x-coordinate
    3361           REAL(wp) ::  x_s !< x-coordinate (shifted)
    3362           REAL(wp) ::  y   !< y-coordinate
    3363           REAL(wp) ::  y_s !< y-coordinate (shifted)
    3364        END TYPE coord
    3365 
    3366        TYPE(coord), DIMENSION(:), ALLOCATABLE, TARGET ::  path     !< path array
    3367        TYPE(coord), DIMENSION(:), ALLOCATABLE, TARGET ::  tmp_path !< temporary path for resizing
    3368 
    3369        CALL cpu_log( log_point_s(20), 'mas_nav_find_path', 'start' )
    3370        node_counter = 0
    3371        a_x = agents(i_ag)%x
    3372        a_y = agents(i_ag)%y
    3373        t_x = agents(i_ag)%t_x
    3374        t_y = agents(i_ag)%t_y
    3375 
    3376 !
    3377 !--    Create temporary navigation mesh including agent and target positions
    3378        CALL mas_nav_create_tmp_mesh( a_x, a_y, t_x, t_y, som )
    3379        tmp_mesh(som)%cost_so_far = 0.0_wp
    3380 !
    3381 !--    Initialize priority queue
    3382        heap_count = 0_iwp
    3383        ALLOCATE(queue(0:100))
    3384        target_reached = .FALSE.
    3385 !
    3386 !--    Add starting point (agent position) to frontier (the frontier consists
    3387 !--    of all the nodes that are to be visited. The node with the smallest
    3388 !--    priority will be visited first. The priority consists of the distance
    3389 !--    from the start node to this node plus a minimal guess (direct distance)
    3390 !--    from this node to the goal). For the starting node, the priority is set
    3391 !--    to 0, as it's the only node thus far
    3392        CALL mas_heap_insert_item(som,0.0_wp)
    3393        cur_node = som
    3394        DO WHILE ( heap_count > 0 )
    3395 !
    3396 !--       Step one: Pick lowest priority item from queue
    3397           node_counter = node_counter + 1
    3398           CALL mas_heap_extract_item(cur_node)
    3399 !
    3400 !--       Node 0 is the goal node
    3401           IF ( cur_node == 0 ) THEN
    3402              EXIT
     3509       INTEGER(iwp) ::  nl            !< local agent counter
     3510       INTEGER(iwp) ::  il            !< local counter
     3511       INTEGER(iwp) ::  jl            !< local counter
     3512       INTEGER(iwp) ::  kl            !< local counter
     3513       INTEGER(iwp) ::  nsteps_total  !< number of steps on path
     3514       INTEGER(iwp) ::  nsteps_dummy  !< number of steps on path
     3515       
     3516       REAL(wp), DIMENSION(0:30) ::  ld_path_x !< local dummy agent path to target (x)
     3517       REAL(wp), DIMENSION(0:30) ::  ld_path_y !< local dummy agent path to target (y)
     3518!
     3519!--    Initialize agent path arrays
     3520       agents(nl)%path_x    = -1
     3521       agents(nl)%path_y    = -1
     3522       agents(nl)%path_x(0) = agents(nl)%x
     3523       agents(nl)%path_y(0) = agents(nl)%y
     3524!
     3525!--    Calculate initial path
     3526       CALL mas_nav_a_star( agents(nl)%x,   agents(nl)%y,                      &
     3527                            agents(nl)%t_x, agents(nl)%t_y, nsteps_total )
     3528!
     3529!--    Set the rest of the agent path that was just calculated
     3530       agents(nl)%path_x(1:nsteps_total) = dummy_path_x(1:nsteps_total)
     3531       agents(nl)%path_y(1:nsteps_total) = dummy_path_y(1:nsteps_total)
     3532!
     3533!--    Iterate through found path and check more intermittent targets need
     3534!--    to be added. For this, run pathfinding between every two consecutive
     3535!--    intermittent targets.
     3536       DO il = 0, MIN(agt_path_size-1, nsteps_total-1)
     3537!
     3538!--       pathfinding between two consecutive intermittent targets
     3539          CALL mas_nav_a_star( agents(nl)%path_x(il),   agents(nl)%path_y(il), &
     3540                              agents(nl)%path_x(il+1), agents(nl)%path_y(il+1),&
     3541                              nsteps_dummy )
     3542          nsteps_dummy = nsteps_dummy - 1
     3543!
     3544!--       If additional intermittent targets are found, add them to the path
     3545          IF ( nsteps_dummy > 0 ) THEN
     3546             ld_path_x = -1
     3547             ld_path_y = -1
     3548             ld_path_x(il+1:il+nsteps_dummy) = dummy_path_x(1:nsteps_dummy)
     3549             ld_path_y(il+1:il+nsteps_dummy) = dummy_path_y(1:nsteps_dummy)
     3550             kl = 1
     3551             DO jl = il+1,nsteps_total
     3552               ld_path_x( il+nsteps_dummy+kl ) = agents(nl)%path_x(jl)
     3553               ld_path_y( il+nsteps_dummy+kl ) = agents(nl)%path_y(jl)
     3554               kl = kl + 1
     3555               IF ( kl > agt_path_size ) EXIT
     3556             ENDDO
     3557             nsteps_total = MIN(nsteps_total + nsteps_dummy, agt_path_size)
     3558             agents(nl)%path_x(il+1:nsteps_total) = ld_path_x(il+1:nsteps_total)
     3559             agents(nl)%path_y(il+1:nsteps_total) = ld_path_y(il+1:nsteps_total)
    34033560          ENDIF
    3404 !
    3405 !--       Loop over all of cur_node's neighbors
    3406           DO il = 1, tmp_mesh(cur_node)%noc
    3407              neigh_node = tmp_mesh(cur_node)%connected_vertices(il)
    3408 !
    3409 !--          Check, if the way from the start node to this neigh_node via
    3410 !--          cur_node is shorter than the previously found shortest path to it.
    3411 !--          If so, replace said cost and add neigh_node to the frontier.
    3412 !--          cost_so_far is initialized as 1.d12 so that all found distances
    3413 !--          should be smaller.
    3414              new_cost   = tmp_mesh(cur_node)%cost_so_far                       &
    3415                          + tmp_mesh(cur_node)%distance_to_vertex(il)
    3416              IF ( new_cost < tmp_mesh(neigh_node)%cost_so_far ) THEN
    3417                 tmp_mesh(neigh_node)%cost_so_far = new_cost
    3418                 tmp_mesh(neigh_node)%origin_id   = cur_node
    3419 !
    3420 !--             Priority in the queue is cost_so_far + heuristic to goal
    3421                 new_priority = new_cost                                        &
    3422                               + heuristic(tmp_mesh(neigh_node)%x,              &
    3423                                 tmp_mesh(neigh_node)%y, tmp_mesh(0)%x,         &
    3424                                 tmp_mesh(0)%y)
    3425                 CALL mas_heap_insert_item(neigh_node,new_priority)
    3426              ENDIF
    3427           ENDDO
     3561
    34283562       ENDDO
    34293563!
    3430 !--    Pathfinding is done. Add nodes to a path array, then give the
    3431 !--    first 15 intermittent targets to the agent.
    3432        DEALLOCATE(queue)
    3433        cur_node = 0
    3434        steps = 0
    3435        ALLOCATE(path(1:100))
    3436        DO WHILE ( cur_node /= -1 )
    3437           steps = steps + 1
    3438 !
    3439 !--       Resize path array if necessary
    3440           IF ( steps > SIZE(path) ) THEN
    3441              ALLOCATE(tmp_path(1:steps-1))
    3442              tmp_path(1:steps-1) = path(1:steps-1)
    3443              DEALLOCATE(path)
    3444              ALLOCATE(path(1:2*(steps-1)))
    3445              path(1:steps-1) = tmp_path(1:steps-1)
    3446              DEALLOCATE(tmp_path)
    3447           ENDIF
    3448           path(steps)%x = tmp_mesh(cur_node)%x
    3449           path(steps)%y = tmp_mesh(cur_node)%y
    3450           path(steps)%x_s = tmp_mesh(cur_node)%x_s
    3451           path(steps)%y_s = tmp_mesh(cur_node)%y_s
    3452           cur_node = tmp_mesh(cur_node)%origin_id
    3453        ENDDO
    3454 !
    3455 !--    Add calculated intermittent targets to agent path until either the
    3456 !--    target or the maximum number of intermittent targets is reached.
    3457 !--    Ignore starting point (reduce index by one), it is agent position.
    3458        path_ag = 1
    3459        steps = steps - 1
    3460        DO WHILE( steps > 0 .AND. path_ag < SIZE(agents(i_ag)%path_x) )
    3461 !
    3462 !--       Each target point is randomly chosen along a line target along the
    3463 !--       bisector of the building corner that starts at corner_gate_start
    3464 !--       and has a width of corner_gate_width. This is to avoid clustering
    3465 !--       when opposing agent groups try to reach the same corner target.
    3466           rn_gate = random_function(iran_agent) * corner_gate_width            &
    3467                                                 + corner_gate_start
    3468           agents(i_ag)%path_x(path_ag) = path(steps)%x + rn_gate               &
    3469                                          * (path(steps)%x_s - path(steps)%x)
    3470           agents(i_ag)%path_y(path_ag) = path(steps)%y + rn_gate               &
    3471                                          * (path(steps)%y_s - path(steps)%y)
    3472           steps = steps - 1
    3473           path_ag = path_ag + 1
    3474        ENDDO
    3475 !
    3476 !--    Set current intermittent target of this agent
    3477        agents(i_ag)%path_counter = 1
    3478        DEALLOCATE(tmp_mesh, path)
    3479        CALL cpu_log( log_point_s(20), 'mas_nav_find_path', 'stop' )
     3564!--    reset path counter to first intermittent target
     3565       agents(nl)%path_counter = 1
    34803566
    34813567    END SUBROUTINE mas_nav_find_path
  • palm/trunk/SOURCE/netcdf_interface_mod.f90

    r3165 r3187  
    2525! -----------------
    2626! $Id$
     27! Changed agent output to precision NF90_DOUBLE
     28!
     29! 3165 2018-07-24 13:12:42Z sward
    2730! Added agent ID output
    2831!
     
    19921995             CALL netcdf_create_var( id_set_agt, (/ id_dim_agtnum,             &
    19931996                                     id_dim_time_agt /), agt_var_names(i),     &
    1994                                      nc_precision(8), id_var_agt(i),           &
     1997                                     NF90_DOUBLE, id_var_agt(i),           &
    19951998                                     TRIM( agt_var_units(i) ),                 &
    19961999                                     TRIM( agt_var_names(i) ), 339, 340, 341 )
Note: See TracChangeset for help on using the changeset viewer.