Mercurial Hosting > traffic-intelligence

comparison python/moving.py @ 573:cae4e5f3fe9f

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
fixed and simplified getSYfromXY
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Tue, 12 Aug 2014 17:47:16 -0400
parents a9c1d61a89b4
children e24eeb244698
comparison
equal deleted inserted replaced
572:9c429c7efe89 573:cae4e5f3fe9f
177 def __sub__(self, other): 177 def __sub__(self, other):
178 return Point(self.x-other.x, self.y-other.y) 178 return Point(self.x-other.x, self.y-other.y)
179 179
180 def __neg__(self): 180 def __neg__(self):
181 return Point(-self.x, -self.y) 181 return Point(-self.x, -self.y)
182
183 def orthogonal(self):
184 return Point(self.y, -self.x)
182 185
183 def multiply(self, alpha): 186 def multiply(self, alpha):
187 'Warning, returns a new Point'
184 return Point(self.x*alpha, self.y*alpha) 188 return Point(self.x*alpha, self.y*alpha)
185 189
186 def plot(self, options = 'o', **kwargs): 190 def plot(self, options = 'o', **kwargs):
187 from matplotlib.pylab import plot 191 from matplotlib.pylab import plot
188 plot([self.x], [self.y], options, **kwargs) 192 plot([self.x], [self.y], options, **kwargs)
240 xinters = (self.y-p1[1])*(p2[0]-p1[0])/(p2[1]-p1[1])+p1[0]; 244 xinters = (self.y-p1[1])*(p2[0]-p1[0])/(p2[1]-p1[1])+p1[0];
241 if p1[0] == p2[0] or self.x <= xinters: 245 if p1[0] == p2[0] or self.x <= xinters:
242 counter+=1; 246 counter+=1;
243 p1=p2 247 p1=p2
244 return (counter%2 == 1); 248 return (counter%2 == 1);
249
250 @staticmethod
251 def fromList(p):
252 return Point(p[0], p[1])
245 253
246 @staticmethod 254 @staticmethod
247 def dot(p1, p2): 255 def dot(p1, p2):
248 'Scalar product' 256 'Scalar product'
249 return p1.x*p2.x+p1.y*p2.y 257 return p1.x*p2.x+p1.y*p2.y
360 print('Error: Division by zero in ppldb2p. Please report this error with the full traceback:') 368 print('Error: Division by zero in ppldb2p. Please report this error with the full traceback:')
361 print('qx={0}, qy={1}, p0x={2}, p0y={3}, p1x={4}, p1y={5}...'.format(qx, qy, p0x, p0y, p1x, p1y)) 369 print('qx={0}, qy={1}, p0x={2}, p0y={3}, p1x={4}, p1y={5}...'.format(qx, qy, p0x, p0y, p1x, p1y))
362 import pdb; pdb.set_trace() 370 import pdb; pdb.set_trace()
363 return X,Y 371 return X,Y
364 372
365 def getSYfromXY(qx, qy, splines, spline_assumption_threshold=0.5, mode=0): 373 def getSYfromXY(qx, qy, splines, goodEnoughSplineDistance = 0.5):
366 ''' Snap a point (coordinates qx, qy) to it's nearest subsegment of it's nearest spline (from the list splines). 374 ''' Snap a point (coordinates qx, qy) to it's nearest subsegment of it's nearest spline (from the list splines).
367 375
368 To force snapping to a single spline, pass the spline alone through splines (e.g. splines=[splines[splineNum]]). 376 To force snapping to a single spline, pass the spline alone through splines (e.g. splines=[splines[splineNum]]).
369 377
370 Output: 378 Output:
379 ''' 387 '''
380 388
381 #(buckle in, it gets ugly from here on out) 389 #(buckle in, it gets ugly from here on out)
382 ss_spline_d = subsec_spline_dist(splines) 390 ss_spline_d = subsec_spline_dist(splines)
383 391
384 temp_dist_min = float('inf') 392 minOffsetY = float('inf')
385 #For each spline 393 #For each spline
386 for spline in range(len(splines)): 394 for spline in range(len(splines)):
387 #For each spline point 395 #For each spline point
388 for spline_p in range(len(splines[spline])): 396 for spline_p in range(len(splines[spline])-1):
389 if(spline_p > (len(splines[spline]) - 2)): 397 #Get closest point on spline
390 break
391 #Get point-intersection distance
392 X,Y = ppldb2p(qx,qy,splines[spline][spline_p][0],splines[spline][spline_p][1],splines[spline][spline_p+1][0],splines[spline][spline_p+1][1]) 398 X,Y = ppldb2p(qx,qy,splines[spline][spline_p][0],splines[spline][spline_p][1],splines[spline][spline_p+1][0],splines[spline][spline_p+1][1])
393 if(X == False and Y == False): 399 if X == False and Y == False:
394 print('Error: Spline {0}, segment {1} has identical bounds and therefore is not a vector. Projection cannot continue.'.format(spline, spline_p)) 400 print('Error: Spline {0}, segment {1} has identical bounds and therefore is not a vector. Projection cannot continue.'.format(spline, spline_p))
395 return [False,False,False,False,False,False,False] 401 return None
396 #Check to see if point is not contained by subspline 402 if utils.inBetween(splines[spline][spline_p][0], splines[spline][spline_p+1][0], X) and utils.inBetween(splines[spline][spline_p][1], splines[spline][spline_p+1][1], Y):
397 if ss_spline_d[spline][0][spline_p]*1.05 < max(utils.pointDistanceL2(splines[spline][spline_p][0],splines[spline][spline_p][1],X,Y),utils.pointDistanceL2(splines[spline][spline_p+1][0],splines[spline][spline_p+1][1],X,Y)): continue 403 offsetY = utils.pointDistanceL2(qx,qy,X,Y)
398 404 if offsetY < minOffsetY:
399 #Ok 405 minOffsetY = offsetY
400 temp_dist = utils.pointDistanceL2(qx,qy,X,Y) 406 snappedSpline = spline
401 if(temp_dist < temp_dist_min): 407 snappedSplineLeadingPoint = spline_p
402 temp_dist_min = temp_dist 408 snapped_x = X
403 snappedSpline = spline 409 snapped_y = Y
404 snappedSplineLeadingPoint = spline_p 410 #Jump loop if significantly close
405 snapped_x = X 411 if offsetY < goodEnoughSplineDistance:
406 snapped_y = Y 412 break
407 #Jump loop if significantly close 413 #Get sub-segment distance
408 if(temp_dist < spline_assumption_threshold): break 414 subsegmentDistance = utils.pointDistanceL2(splines[snappedSpline][snappedSplineLeadingPoint][0],splines[snappedSpline][snappedSplineLeadingPoint][1],snapped_x,snapped_y)
409 415 #Get total segment distance
410 try: 416 splineDistanceS = ss_spline_d[snappedSpline][1][snappedSplineLeadingPoint] + subsegmentDistance
411 #Get sub-segment distance 417 orthogonalSplineVector = Point(splines[snappedSpline][snappedSplineLeadingPoint+1][1]-splines[snappedSpline][snappedSplineLeadingPoint][1],
412 subsegmentDistance = utils.pointDistanceL2(splines[snappedSpline][snappedSplineLeadingPoint][0],splines[snappedSpline][snappedSplineLeadingPoint][1],snapped_x,snapped_y) 418 splines[snappedSpline][snappedSplineLeadingPoint][0]-splines[snappedSpline][snappedSplineLeadingPoint+1][0])#positive orthogonal vector of vector (x,y) is (y, -x)
413 #Get total segment distance 419 offsetVector = Point(qx-snapped_x, qy-snapped_y)
414 splineDistanceS = ss_spline_d[snappedSpline][1][snappedSplineLeadingPoint] + subsegmentDistance 420 if Point.dot(orthogonalSplineVector, offsetVector) < 0:
415 #Get offset distance 421 minOffsetY = -minOffsetY
416 offsetY = utils.pointDistanceL2(qx,qy, snapped_x,snapped_y) 422 return [snappedSpline, snappedSplineLeadingPoint, snapped_x, snapped_y, subsegmentDistance, splineDistanceS, minOffsetY]
417
418 if(mode):
419 direction = getOrientation([splines[snappedSpline][snappedSplineLeadingPoint][0],splines[snappedSpline][snappedSplineLeadingPoint][1]] , [splines[snappedSpline][snappedSplineLeadingPoint+1][0],splines[snappedSpline][snappedSplineLeadingPoint+1][1]] , [qx,qy])
420 if(direction == 'left'): offsetY = -offsetY
421
422 return [snappedSpline, snappedSplineLeadingPoint, snapped_x, snapped_y, subsegmentDistance, splineDistanceS, offsetY]
423 except:
424 return [False,False,False,False,False,False,False]
425
426 423
427 def getXYfromSY(s, y, splineNum, splines, mode = 0): 424 def getXYfromSY(s, y, splineNum, splines, mode = 0):
428 ''' Find X,Y coordinate from S,Y data. 425 ''' Find X,Y coordinate from S,Y data.
429 if mode = 0 : return Snapped X,Y 426 if mode = 0 : return Snapped X,Y
430 if mode !=0 : return Real X,Y 427 if mode !=0 : return Real X,Y