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