forked from dslwz2008/SocialForceModel
-
Notifications
You must be signed in to change notification settings - Fork 2
/
Copy pathsimulator_WP_Doorway.py
147 lines (115 loc) · 3.99 KB
/
simulator_WP_Doorway.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
# -*-coding:utf-8-*-
# Author: SS and WP
import pygame
import pygame.draw
import numpy as np
from agent_WP_Random import *
from tools import *
#from config import *
#import random
SCREENSIZE = [800, 400]
RESOLUTION = 180
AGENTSNUM = 8
BACKGROUNDCOLOR = [255,255,255]
AGENTCOLOR = [0,0,255]
LINECOLOR = [255,0,0]
AGENTSIZE = 9
AGENTSICKNESS = 3
#WALLSFILE = "walls.csv"
pygame.init()
screen = pygame.display.set_mode(SCREENSIZE)
pygame.display.set_caption('Social Force Model - Single-Room Egress')
clock = pygame.time.Clock()
# initialize walls
#walls = []
#for line in open(WALLSFILE):
# coords = line.split(',')
# wall = []
# wall.append(float(coords[0]))
# wall.append(float(coords[1]))
# wall.append(float(coords[2]))
# wall.append(float(coords[3]))
# walls.append(wall)
walls = [[3.33, 3.33, 29.97, 3.33],
[3.33, 3.33, 3.33, 33.31],
[3.33, 33.31, 29.97, 33.31],
[23.31, 3.33, 33.31, 14.02],
[33.31, 20.92, 23.31, 33.31]]
print(walls)
# initialize agents
agents = []
for n in range(AGENTSNUM):
agent = Agent()
agents.append(agent)
running = True
while running:
for event in pygame.event.get():
if event.type == pygame.QUIT:
running = False
elif event.type == pygame.MOUSEBUTTONDOWN:
(mouseX, mouseY) = pygame.mouse.get_pos()
# elif event.type == pygame.MOUSEBUTTONUP:
screen.fill(BACKGROUNDCOLOR)
# draw walls
for wall in walls:
startPos = np.array([wall[0],wall[1]])
endPos = np.array([wall[2],wall[3]])
startPx = startPos*10 #worldCoord2ScreenCoord(startPos,SCREENSIZE,RESOLUTION)
endPx = endPos*10 #worldCoord2ScreenCoord(endPos,SCREENSIZE,RESOLUTION)
pygame.draw.line(screen, LINECOLOR,startPx,endPx)
# draw agents
# pygame.draw.circle(screen, AGENTCOLOR, (np.array(SCREENSIZE)/2).tolist(),
# AGENTSIZE, AGENTSICKNESS)
# 计算相互作用力
for idai,ai in enumerate(agents):
# 初始速度和位置
#v0 = ai.actualV
#r0 = ai.pos
ai.direction = normalize(ai.dest - ai.pos)
ai.desiredV = ai.desiredSpeed*ai.direction
# 计算受力
adapt = ai.adaptVel()
peopleInter = 0.0
wallInter = 0.0
for idaj,aj in enumerate(agents):
if idai == idaj:
continue
peopleInter += ai.peopleInteraction(aj)
for wall in walls:
wallInter += ai.wallInteraction(wall)
#print('Forces from Walls:', wallInter)
#print('Forces from people:', peopleInter)
sumForce = adapt + peopleInter + wallInter
# 计算加速度
accl = sumForce/ai.mass
# 计算速度
ai.actualV = ai.actualV + accl*0.5 # consider dt = 0.5
# 计算位移
ai.pos = ai.pos + ai.actualV*0.5
#ai.pos = r0 + v0 + 0.5*accl
#print(ai.pos)
#print(accl,ai.actualV,ai.pos)
if (ai.pos[0] >= 35.0) & (ai.Goal == 0):
print('test')
ai.Goal = 1
ai.timeOut = pygame.time.get_ticks()
#ai.timeOut = clock.get_time()/1000.0
print('Time to Reach the Goal:', ai.timeOut)
for agent in agents:
#scPos = agent.pos*10 #worldCoord2ScreenCoord(agent.pos, SCREENSIZE, RESOLUTION)
scPos = [0, 0]
scPos[0] = int(agent.pos[0]*10) #worldCoord2ScreenCoord(agent.pos, SCREENSIZE, RESOLUTION)
scPos[1] = int(agent.pos[1]*10)
endPos = [0, 0]
endPos[0] = int(agent.pos[0]*10 + agent.actualV[0]*10)
endPos[1] = int(agent.pos[1]*10 + agent.actualV[1]*10)
endPosDV = [0, 0]
endPosDV[0] = int(agent.pos[0]*10 + agent.desiredV[0]*10)
endPosDV[1] = int(agent.pos[1]*10 + agent.desiredV[1]*10)
pygame.draw.circle(screen, AGENTCOLOR, scPos, AGENTSIZE, AGENTSICKNESS)
pygame.draw.line(screen, AGENTCOLOR, scPos, endPos, 2)
pygame.draw.line(screen, [255,60,0], scPos, endPosDV, 2)
#print(scPos)
pygame.display.flip()
clock.tick(20)
#clock.get_time