Mercurial Hosting > traffic-intelligence
comparison python/extrapolation.py @ 242:942455aff829
merged with the addition of Mohamed's code
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Fri, 13 Jul 2012 17:30:25 -0400 |
parents | d2b68111f87e |
children | e0988a8ace0c |
comparison
equal
deleted
inserted
replaced
241:ee1caff48b03 | 242:942455aff829 |
---|---|
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 |