Mercurial Hosting > traffic-intelligence
comparison python/moving.py @ 569:0057c04f94d5
work in progress on intersections (for PET)
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Wed, 06 Aug 2014 17:53:38 -0400 |
parents | 072cedc3f33d |
children | 5adaab9ad160 |
comparison
equal
deleted
inserted
replaced
567:072cedc3f33d | 569:0057c04f94d5 |
---|---|
307 prepared_polygon = prep(polygon) | 307 prepared_polygon = prep(polygon) |
308 return filter(prepared_polygon.contains, points) | 308 return filter(prepared_polygon.contains, points) |
309 | 309 |
310 # Functions for coordinate transformation | 310 # Functions for coordinate transformation |
311 # From Paul St-Aubin's PVA tools | 311 # From Paul St-Aubin's PVA tools |
312 def ppd(*args): | |
313 ''' Get point-to-point distance. | |
314 | |
315 Example usage: | |
316 ============== | |
317 d = ppd(x1,y1,x2,y2) | |
318 d = ppd(P1,P2) | |
319 ''' | |
320 | |
321 if(len(args)==4): [x1,y1,x2,y2] = [args[0],args[1],args[2],args[3]] | |
322 elif(len(args)==2 and hasattr(args[0], '__iter__')): [x1,y1,x2,y2] = [args[0][0],args[0][1],args[1][0],args[1][1]] | |
323 else: raise Exception("Library error: ppd() in tools_geo takes exactly 2 or 4 arguments.") | |
324 return sqrt((x2-x1)**2+(y2-y1)**2) | |
325 | |
326 def subsec_spline_dist(splines): | 312 def subsec_spline_dist(splines): |
327 ''' Prepare list of spline subsegments from a spline list. | 313 ''' Prepare list of spline subsegments from a spline list. |
328 | 314 |
329 Output: | 315 Output: |
330 ======= | 316 ======= |
345 ss_spline_d[spline][1] = zeros(len(splines[spline])-1) #Cumulative distance | 331 ss_spline_d[spline][1] = zeros(len(splines[spline])-1) #Cumulative distance |
346 ss_spline_d[spline][2] = zeros(len(splines[spline])) #Cumulative distance with trailing distance | 332 ss_spline_d[spline][2] = zeros(len(splines[spline])) #Cumulative distance with trailing distance |
347 for spline_p in range(len(splines[spline])): | 333 for spline_p in range(len(splines[spline])): |
348 if spline_p > (len(splines[spline]) - 2): | 334 if spline_p > (len(splines[spline]) - 2): |
349 break | 335 break |
350 ss_spline_d[spline][0][spline_p] = ppd(splines[spline][spline_p][0],splines[spline][spline_p][1],splines[spline][(spline_p+1)][0],splines[spline][(spline_p+1)][1]) # TODO use points and remove ppd | 336 ss_spline_d[spline][0][spline_p] = utils.pointDistanceL2(splines[spline][spline_p][0],splines[spline][spline_p][1],splines[spline][(spline_p+1)][0],splines[spline][(spline_p+1)][1]) |
351 ss_spline_d[spline][1][spline_p] = sum(ss_spline_d[spline][0][0:spline_p]) | 337 ss_spline_d[spline][1][spline_p] = sum(ss_spline_d[spline][0][0:spline_p]) |
352 ss_spline_d[spline][2][spline_p] = sum(ss_spline_d[spline][0][0:spline_p]) | 338 ss_spline_d[spline][2][spline_p] = sum(ss_spline_d[spline][0][0:spline_p]) |
353 | 339 |
354 ss_spline_d[spline][2][-1] = ss_spline_d[spline][2][-2] + ss_spline_d[spline][0][-1] | 340 ss_spline_d[spline][2][-1] = ss_spline_d[spline][2][-2] + ss_spline_d[spline][0][-1] |
355 | 341 |
388 snapped x coordinate, | 374 snapped x coordinate, |
389 snapped y coordinate, | 375 snapped y coordinate, |
390 subsegment distance, | 376 subsegment distance, |
391 spline distance, | 377 spline distance, |
392 orthogonal point offset] | 378 orthogonal point offset] |
393 ''' | 379 ''' |
394 | 380 |
395 #(buckle in, it gets ugly from here on out) | 381 #(buckle in, it gets ugly from here on out) |
396 ss_spline_d = subsec_spline_dist(splines) | 382 ss_spline_d = subsec_spline_dist(splines) |
397 | 383 |
398 temp_dist_min = float('inf') | 384 temp_dist_min = float('inf') |
406 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]) | 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]) |
407 if(X == False and Y == False): | 393 if(X == False and Y == False): |
408 print('Error: Spline {0}, segment {1} has identical bounds and therefore is not a vector. Projection cannot continue.'.format(spline, spline_p)) | 394 print('Error: Spline {0}, segment {1} has identical bounds and therefore is not a vector. Projection cannot continue.'.format(spline, spline_p)) |
409 return [False,False,False,False,False,False,False] | 395 return [False,False,False,False,False,False,False] |
410 #Check to see if point is not contained by subspline | 396 #Check to see if point is not contained by subspline |
411 if ss_spline_d[spline][0][spline_p]*1.05 < max(ppd(splines[spline][spline_p][0],splines[spline][spline_p][1],X,Y),ppd(splines[spline][spline_p+1][0],splines[spline][spline_p+1][1],X,Y)): continue | 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 |
412 | 398 |
413 #Ok | 399 #Ok |
414 temp_dist = ppd(qx,qy,X,Y) | 400 temp_dist = utils.pointDistanceL2(qx,qy,X,Y) |
415 if(temp_dist < temp_dist_min): | 401 if(temp_dist < temp_dist_min): |
416 temp_dist_min = temp_dist | 402 temp_dist_min = temp_dist |
417 snappedSpline = spline | 403 snappedSpline = spline |
418 snappedSplineLeadingPoint = spline_p | 404 snappedSplineLeadingPoint = spline_p |
419 snapped_x = X | 405 snapped_x = X |
421 #Jump loop if significantly close | 407 #Jump loop if significantly close |
422 if(temp_dist < spline_assumption_threshold): break | 408 if(temp_dist < spline_assumption_threshold): break |
423 | 409 |
424 try: | 410 try: |
425 #Get sub-segment distance | 411 #Get sub-segment distance |
426 subsegmentDistance = ppd(splines[snappedSpline][snappedSplineLeadingPoint][0],splines[snappedSpline][snappedSplineLeadingPoint][1],snapped_x,snapped_y) | 412 subsegmentDistance = utils.pointDistanceL2(splines[snappedSpline][snappedSplineLeadingPoint][0],splines[snappedSpline][snappedSplineLeadingPoint][1],snapped_x,snapped_y) |
427 #Get total segment distance | 413 #Get total segment distance |
428 splineDistanceS = ss_spline_d[snappedSpline][1][snappedSplineLeadingPoint] + subsegmentDistance | 414 splineDistanceS = ss_spline_d[snappedSpline][1][snappedSplineLeadingPoint] + subsegmentDistance |
429 #Get offset distance | 415 #Get offset distance |
430 offsetY = ppd(qx,qy, snapped_x,snapped_y) | 416 offsetY = utils.pointDistanceL2(qx,qy, snapped_x,snapped_y) |
431 | 417 |
432 if(mode): | 418 if(mode): |
433 direction = getOrientation([splines[snappedSpline][snappedSplineLeadingPoint][0],splines[snappedSpline][snappedSplineLeadingPoint][1]] , [splines[snappedSpline][snappedSplineLeadingPoint+1][0],splines[snappedSpline][snappedSplineLeadingPoint+1][1]] , [qx,qy]) | 419 direction = getOrientation([splines[snappedSpline][snappedSplineLeadingPoint][0],splines[snappedSpline][snappedSplineLeadingPoint][1]] , [splines[snappedSpline][snappedSplineLeadingPoint+1][0],splines[snappedSpline][snappedSplineLeadingPoint+1][1]] , [qx,qy]) |
434 if(direction == 'left'): offsetY = -offsetY | 420 if(direction == 'left'): offsetY = -offsetY |
435 | 421 |
538 | 524 |
539 @staticmethod | 525 @staticmethod |
540 def similar(f1, f2, maxDistance2, maxDeltavelocity2): | 526 def similar(f1, f2, maxDistance2, maxDeltavelocity2): |
541 return (f1.position-f2.position).norm2Squared()<maxDistance2 and (f1.velocity-f2.velocity).norm2Squared()<maxDeltavelocity2 | 527 return (f1.position-f2.position).norm2Squared()<maxDistance2 and (f1.velocity-f2.velocity).norm2Squared()<maxDeltavelocity2 |
542 | 528 |
543 def intersection(p1, p2, dp1, dp2): | 529 def intersection(p1, p2, p3, p4): |
544 '''Returns the intersection point between the two lines | 530 ''' Intersection point (x,y) of lines formed by the vectors p1-p2 and p3-p4 |
545 defined by the respective vectors (dp) and origin points (p)''' | 531 http://paulbourke.net/geometry/lineline2d/ |
546 from numpy import matrix | 532 |
547 from numpy.linalg import linalg | 533 If these lines are parralel, there will be a division by zero error. |
548 A = matrix([[dp1.y, -dp1.x], | 534 ''' |
549 [dp2.y, -dp2.x]]) | 535 dp12 = p2-p1 |
550 B = matrix([[dp1.y*p1.x-dp1.x*p1.y], | 536 det = ((p4.y-p3.y)*(p2.x-p1.x)-(p4.x-p3.x)*(p2.y-p1.y)) |
551 [dp2.y*p2.x-dp2.x*p2.y]]) | 537 if det == 0: |
552 | |
553 if linalg.det(A) == 0: | |
554 return None | 538 return None |
555 else: | 539 else: |
556 intersection = linalg.solve(A,B) | 540 ua = ((p4.x-p3.x)*(p1.y-p3.y)-(p4.y-p3.y)*(p1.x-p3.x))/det |
557 return Point(intersection[0,0], intersection[1,0]) | 541 dp12.multiply(ua) |
542 #x = p1.x + ua*(p2.x - p1.x) | |
543 #y = p1.y + ua*(p2.y - p1.y) | |
544 return p1+dp12 | |
545 | |
546 # def intersection(p1, p2, dp1, dp2): | |
547 # '''Returns the intersection point between the two lines | |
548 # defined by the respective vectors (dp) and origin points (p)''' | |
549 # from numpy import matrix | |
550 # from numpy.linalg import linalg | |
551 # A = matrix([[dp1.y, -dp1.x], | |
552 # [dp2.y, -dp2.x]]) | |
553 # B = matrix([[dp1.y*p1.x-dp1.x*p1.y], | |
554 # [dp2.y*p2.x-dp2.x*p2.y]]) | |
555 | |
556 # if linalg.det(A) == 0: | |
557 # return None | |
558 # else: | |
559 # intersection = linalg.solve(A,B) | |
560 # return Point(intersection[0,0], intersection[1,0]) | |
558 | 561 |
559 def segmentIntersection(p1, p2, p3, p4): | 562 def segmentIntersection(p1, p2, p3, p4): |
560 '''Returns the intersecting point of the segments [p1, p2] and [p3, p4], None otherwise''' | 563 '''Returns the intersecting point of the segments [p1, p2] and [p3, p4], None otherwise''' |
561 | 564 |
562 if (Interval.intersection(Interval(p1.x,p2.x,True), Interval(p3.x,p4.x,True)).empty()) or (Interval.intersection(Interval(p1.y,p2.y,True), Interval(p3.y,p4.y,True)).empty()): | 565 if (Interval.intersection(Interval(p1.x,p2.x,True), Interval(p3.x,p4.x,True)).empty()) or (Interval.intersection(Interval(p1.y,p2.y,True), Interval(p3.y,p4.y,True)).empty()): |
563 return None | 566 return None |
564 else: | 567 else: |
565 dp1 = p2-p1 | 568 #dp1 = p2-p1 |
566 dp3 = p4-p3 | 569 #dp3 = p4-p3 |
567 inter = intersection(p1, p3, dp1, dp3) | 570 inter = intersection(p1, p2, p3, p4)#(p1, p3, dp1, dp3) |
568 if (inter != None | 571 if (inter != None |
569 and utils.inBetween(p1.x, p2.x, inter.x) | 572 and utils.inBetween(p1.x, p2.x, inter.x) |
570 and utils.inBetween(p3.x, p4.x, inter.x) | 573 and utils.inBetween(p3.x, p4.x, inter.x) |
571 and utils.inBetween(p1.y, p2.y, inter.y) | 574 and utils.inBetween(p1.y, p2.y, inter.y) |
572 and utils.inBetween(p3.y, p4.y, inter.y)): | 575 and utils.inBetween(p3.y, p4.y, inter.y)): |