|
240
|
1 ##########
|
|
|
2 # Extrapolation Hypothesis
|
|
|
3 ##########
|
|
|
4
|
|
|
5 import sys
|
|
|
6
|
|
|
7 sys.path.append("G:/0-phdstart/Code/traffic-intelligence1/python")
|
|
|
8
|
|
|
9 import moving
|
|
|
10
|
|
|
11 #Default values
|
|
|
12 FPS= 25 # No. of frame per second (FPS)
|
|
|
13 vLimit= 25/FPS #assume limit speed is 90km/hr = 25 m/sec
|
|
|
14 deltaT= FPS*5 # extrapolatation time Horizon = 5 second
|
|
|
15
|
|
|
16 def ExtrapolationPosition (movingObject1, instant,deltaT):
|
|
|
17 ''' extrapolation hypothesis: constant velocity'''
|
|
|
18 return movingObject1.getPositionAtInstant(instant) + movingObject1.getVelocityAtInstant(instant). multiply(deltaT)
|
|
|
19
|
|
|
20 def motion (position, velocity, acceleration):
|
|
|
21 ''' extrapolation hypothesis: constant acceleration'''
|
|
|
22 from math import atan2,cos,sin
|
|
|
23 vInit= velocity
|
|
|
24 vInitial= velocity.norm2()
|
|
|
25 theta= atan2(velocity.y,velocity.x)
|
|
|
26 vFinal= vInitial+acceleration
|
|
|
27
|
|
|
28 if acceleration<= 0:
|
|
|
29 v= max(0,vFinal)
|
|
|
30 velocity= moving.Point(v* cos(theta),v* sin(theta))
|
|
|
31 position= position+ (velocity+vInit). multiply(0.5)
|
|
|
32 else:
|
|
|
33 v= min(vLimit,vFinal)
|
|
|
34 velocity= moving.Point(v* cos(theta),v* sin(theta))
|
|
|
35 position= position+ (velocity+vInit). multiply(0.5)
|
|
|
36 return(position,velocity)
|
|
|
37
|
|
|
38 def motionPET (position, velocity, acceleration, deltaT):
|
|
|
39 ''' extrapolation hypothesis: constant acceleration for calculating pPET '''
|
|
|
40 from math import atan2,cos,sin,fabs
|
|
|
41 vInit= velocity
|
|
|
42 vInitial= velocity.norm2()
|
|
|
43 theta= atan2(velocity.y,velocity.x)
|
|
|
44 vFinal= vInitial+acceleration * deltaT
|
|
|
45 if acceleration< 0:
|
|
|
46 if vFinal> 0:
|
|
|
47 velocity= moving.Point(vFinal* cos(theta),vFinal* sin(theta))
|
|
|
48 position= position+ (vInit+ velocity). multiply(0.5*deltaT)
|
|
|
49 else:
|
|
|
50 T= fabs(vInitial/acceleration)
|
|
|
51 position= position + vInit. multiply(0.5*T)
|
|
|
52 elif acceleration> 0 :
|
|
|
53 if vFinal<= vLimit:
|
|
|
54 velocity= moving.Point(vFinal* cos(theta),vFinal* sin(theta))
|
|
|
55 position= position+ (vInit+ velocity). multiply(0.5*deltaT)
|
|
|
56 else:
|
|
|
57 time1= fabs((vLimit-vInitial)/acceleration)
|
|
|
58 velocity= moving.Point(vLimit* cos(theta),vLimit* sin(theta))
|
|
|
59 position= (position+ (velocity+vInit). multiply(0.5*time1)) + (velocity.multiply (deltaT-time1))
|
|
|
60 elif acceleration == 0:
|
|
|
61 position= position + velocity. multiply(deltaT)
|
|
|
62
|
|
|
63 return position
|
|
|
64
|
|
|
65 def timePET (position, velocity, acceleration, intersectedPoint ):
|
|
|
66 ''' extrapolation hypothesis: constant acceleration for calculating pPET '''
|
|
|
67 from math import atan2,cos,sin,fabs
|
|
|
68 vInit= velocity
|
|
|
69 vInitial= velocity.norm2()
|
|
|
70 theta= atan2(velocity.y,velocity.x)
|
|
|
71 vFinal= vInitial+acceleration * deltaT
|
|
|
72 if acceleration< 0:
|
|
|
73 if vFinal> 0:
|
|
|
74 velocity= moving.Point(vFinal* cos(theta),vFinal* sin(theta))
|
|
|
75 time= fabs((intersectedPoint.x-position.x)/(0.5*(vInit.x+ velocity.x)))
|
|
|
76 else:
|
|
|
77 time= fabs((intersectedPoint.x-position.x)/(0.5*(vInit.x)))
|
|
|
78 elif acceleration> 0 :
|
|
|
79 if vFinal<= vLimit:
|
|
|
80 velocity= moving.Point(vFinal* cos(theta),vFinal* sin(theta))
|
|
|
81 time= fabs((intersectedPoint.x-position.x)/(0.5*(vInit.x+ velocity.x)))
|
|
|
82 else:
|
|
|
83 time1= fabs((vLimit-vInitial)/acceleration)
|
|
|
84 velocity= moving.Point(vLimit* cos(theta),vLimit* sin(theta))
|
|
|
85 time2= fabs((intersectedPoint.x-position.x)/(0.5*(vInit.x+ velocity.x)))
|
|
|
86 if time2<=time1:
|
|
|
87 time= time2
|
|
|
88 else:
|
|
|
89 position2= (position+ (velocity+vInit). multiply(0.5*time1))
|
|
|
90 time= time1+fabs((intersectedPoint.x-position2.x)/( velocity.x))
|
|
|
91 elif acceleration == 0:
|
|
|
92 time= fabs((intersectedPoint.x-position.x)/(velocity.x))
|
|
|
93
|
|
|
94 return time
|
|
|
95
|
|
|
96 def motionSteering (position, velocity, deltaTheta, deltaT ):
|
|
|
97 ''' extrapolation hypothesis: steering with deltaTheta'''
|
|
|
98 from math import atan2,cos,sin
|
|
|
99 vInitial= velocity.norm2()
|
|
|
100 theta= atan2(velocity.y,velocity.x)
|
|
|
101 newTheta= theta + deltaTheta
|
|
|
102 velocity= moving.Point(vInitial* cos(newTheta),vInitial* sin(newTheta))
|
|
|
103 position= position+ (velocity). multiply(deltaT)
|
|
|
104 return position
|
|
|
105
|
|
|
106 def MonteCarlo(movingObject1,movingObject2, instant):
|
|
|
107 ''' Monte Carlo Simulation : estimate the probability of collision'''
|
|
|
108 from random import uniform
|
|
|
109 from math import pow, sqrt, sin, cos,atan2
|
|
|
110 N=1000
|
|
|
111 ProbOfCollision = 0
|
|
|
112 for n in range (1, N):
|
|
|
113 # acceleration limit
|
|
|
114 acc1 = uniform(-0.040444,0)
|
|
|
115 acc2 = uniform(-0.040444,0)
|
|
|
116 p1= movingObject1.getPositionAtInstant(instant)
|
|
|
117 p2= movingObject2.getPositionAtInstant(instant)
|
|
|
118 v1= movingObject1.getVelocityAtInstant(instant)
|
|
|
119 v2= movingObject2.getVelocityAtInstant(instant)
|
|
|
120 distance= (p1-p2).norm2()
|
|
|
121 distanceThreshold= 1.8
|
|
|
122 t=1
|
|
|
123 while distance > distanceThreshold and t <= deltaT:
|
|
|
124 # Extrapolation position
|
|
|
125 (p1,v1) = motion(p1,v1,acc1)
|
|
|
126 (p2,v2) = motion(p2,v2,acc2)
|
|
|
127 distance= (p1-p2).norm2()
|
|
|
128 if distance <=distanceThreshold:
|
|
|
129 ProbOfCollision= ProbOfCollision+1
|
|
|
130 t+=1
|
|
|
131 POC= float(ProbOfCollision)/N
|
|
|
132 return POC
|
|
|
133
|
|
|
134 def velocitySteering(velocity,steering):
|
|
|
135 from math import atan2,cos,sin
|
|
|
136 vInitial= velocity.norm2()
|
|
|
137 theta= atan2(velocity.y,velocity.x)
|
|
|
138 newTheta= theta + steering
|
|
|
139 v= moving.Point(vInitial* cos(newTheta),vInitial* sin(newTheta))
|
|
|
140 return v
|
|
|
141
|
|
|
142 def MonteCarloSteering(movingObject1,movingObject2, instant,steering1,steering2):
|
|
|
143 ''' Monte Carlo Simulation : estimate the probability of collision in case of steering'''
|
|
|
144 from random import uniform
|
|
|
145 from math import pow, sqrt, sin, cos,atan2
|
|
|
146 N=1000
|
|
|
147 L= 2.4
|
|
|
148 ProbOfCollision = 0
|
|
|
149 for n in range (1, N):
|
|
|
150 # acceleration limit
|
|
|
151 acc1 = uniform(-0.040444,0)
|
|
|
152 acc2 = uniform(-0.040444,0)
|
|
|
153 p1= movingObject1.getPositionAtInstant(instant)
|
|
|
154 p2= movingObject2.getPositionAtInstant(instant)
|
|
|
155 vInit1= movingObject1.getVelocityAtInstant(instant)
|
|
|
156 v1= velocitySteering (vInit1,steering1)
|
|
|
157 vInit2= movingObject2.getVelocityAtInstant(instant)
|
|
|
158 v2= velocitySteering (vInit2,steering2)
|
|
|
159 distance= (p1-p2).norm2()
|
|
|
160 distanceThreshold= 1.8
|
|
|
161 t=1
|
|
|
162 while distance > distanceThreshold and t <= deltaT:
|
|
|
163 # Extrapolation position
|
|
|
164 (p1,v1) = motion(p1,v1,acc1)
|
|
|
165 (p2,v2) = motion(p2,v2,acc2)
|
|
|
166 distance= (p1-p2).norm2()
|
|
|
167 if distance <=distanceThreshold:
|
|
|
168 ProbOfCollision= ProbOfCollision+1
|
|
|
169 t+=1
|
|
|
170 POC= float(ProbOfCollision)/N
|
|
|
171 return POC
|
|
|
172
|
|
|
173
|
