Mercurial Hosting > traffic-intelligence
comparison python/moving.py @ 573:cae4e5f3fe9f
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 |