Mercurial > hg > nsaunier > traffic-intelligence
comparison python/moving.py @ 250:59f547aebaac
modified prediction functions, added norm/angle representation of Points
| author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
|---|---|
| date | Mon, 23 Jul 2012 02:50:26 -0400 |
| parents | 99173da7afae |
| children | 13ec22bec5d4 |
comparison
equal
deleted
inserted
replaced
| 249:99173da7afae | 250:59f547aebaac |
|---|---|
| 226 @staticmethod | 226 @staticmethod |
| 227 def plotAll(points, color='r'): | 227 def plotAll(points, color='r'): |
| 228 from matplotlib.pyplot import scatter | 228 from matplotlib.pyplot import scatter |
| 229 scatter([p.x for p in points],[p.y for p in points], c=color) | 229 scatter([p.x for p in points],[p.y for p in points], c=color) |
| 230 | 230 |
| 231 | 231 class NormAngle: |
| 232 def predictPosition(nTimeSteps, initialPosition, initialVelocity, initialAcceleration = Point(0,0)): | 232 'alternate encoding of a point, by its norm and orientation' |
| 233 | |
| 234 def __init__(self, norm, angle): | |
| 235 self.norm = norm | |
| 236 self.angle = angle | |
| 237 | |
| 238 @staticmethod | |
| 239 def fromPoint(p): | |
| 240 from math import atan2 | |
| 241 norm = p.norm2() | |
| 242 if norm > 0: | |
| 243 angle = atan2(p.x, p.y) | |
| 244 return NormAngle(norm, angle) | |
| 245 | |
| 246 def __add__(self, other): | |
| 247 'a norm cannot become negative' | |
| 248 return NormAngle(max(self.norm+other.norm, 0), self.orientation+other.orientation) | |
| 249 | |
| 250 def getPoint(self): | |
| 251 from math import cos, sin | |
| 252 return Point(self.norm*cos(self.orientation), self.norm*sin(self.orientation)) | |
| 253 | |
| 254 | |
| 255 def predictPositionNoLimit(nTimeSteps, initialPosition, initialVelocity, initialAcceleration = Point(0,0)): | |
| 233 '''Predicts the position in nTimeSteps at constant speed/acceleration''' | 256 '''Predicts the position in nTimeSteps at constant speed/acceleration''' |
| 234 return initialPosition+initialVelocity.multiply(nTimeSteps) + initialAcceleration.multiply(nTimeSteps**2) | 257 return initialVelocity + initialAcceleration.multiply(nTimeSteps),initialPosition+initialVelocity.multiply(nTimeSteps) + initialAcceleration.multiply(nTimeSteps**2*0.5) |
| 258 | |
| 259 def predictPosition(position, speedOrientation, control, maxSpeed = None): | |
| 260 '''Predicts the position (moving.Point) at the next time step with given control input (deltaSpeed, deltaTheta) | |
| 261 speedOrientation is the other encoding of velocity, (speed, orientation) | |
| 262 speedOrientation and control are NormAngle''' | |
| 263 predictedSpeedTheta = speedOrientation+control | |
| 264 if maxSpeed: | |
| 265 predictedSpeedTheta.norm = min(predictedSpeedTheta.norm, maxSpeed) | |
| 266 predictedPosition = position+predictedSpeedTheta.getPoint() | |
| 267 return predictedPosition, predictedSpeedTheta | |
| 235 | 268 |
| 236 | 269 |
| 237 class FlowVector: | 270 class FlowVector: |
| 238 '''Class to represent 4-D flow vectors, | 271 '''Class to represent 4-D flow vectors, |
| 239 ie a position and a velocity''' | 272 ie a position and a velocity''' |