Mercurial Hosting > traffic-intelligence
changeset 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 |
files | python/moving.py python/tests/moving.txt python/utils.py |
diffstat | 3 files changed, 57 insertions(+), 42 deletions(-) [+] |
line wrap: on
line diff
--- a/python/moving.py Tue Aug 05 17:45:36 2014 -0400 +++ b/python/moving.py Wed Aug 06 17:53:38 2014 -0400 @@ -309,20 +309,6 @@ # Functions for coordinate transformation # From Paul St-Aubin's PVA tools -def ppd(*args): - ''' Get point-to-point distance. - - Example usage: - ============== - d = ppd(x1,y1,x2,y2) - d = ppd(P1,P2) - ''' - - if(len(args)==4): [x1,y1,x2,y2] = [args[0],args[1],args[2],args[3]] - 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]] - else: raise Exception("Library error: ppd() in tools_geo takes exactly 2 or 4 arguments.") - return sqrt((x2-x1)**2+(y2-y1)**2) - def subsec_spline_dist(splines): ''' Prepare list of spline subsegments from a spline list. @@ -347,7 +333,7 @@ for spline_p in range(len(splines[spline])): if spline_p > (len(splines[spline]) - 2): break - 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 + 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]) ss_spline_d[spline][1][spline_p] = sum(ss_spline_d[spline][0][0:spline_p]) ss_spline_d[spline][2][spline_p] = sum(ss_spline_d[spline][0][0:spline_p]) @@ -390,7 +376,7 @@ subsegment distance, spline distance, orthogonal point offset] - ''' + ''' #(buckle in, it gets ugly from here on out) ss_spline_d = subsec_spline_dist(splines) @@ -408,10 +394,10 @@ print('Error: Spline {0}, segment {1} has identical bounds and therefore is not a vector. Projection cannot continue.'.format(spline, spline_p)) return [False,False,False,False,False,False,False] #Check to see if point is not contained by subspline - 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 + 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 #Ok - temp_dist = ppd(qx,qy,X,Y) + temp_dist = utils.pointDistanceL2(qx,qy,X,Y) if(temp_dist < temp_dist_min): temp_dist_min = temp_dist snappedSpline = spline @@ -423,11 +409,11 @@ try: #Get sub-segment distance - subsegmentDistance = ppd(splines[snappedSpline][snappedSplineLeadingPoint][0],splines[snappedSpline][snappedSplineLeadingPoint][1],snapped_x,snapped_y) + subsegmentDistance = utils.pointDistanceL2(splines[snappedSpline][snappedSplineLeadingPoint][0],splines[snappedSpline][snappedSplineLeadingPoint][1],snapped_x,snapped_y) #Get total segment distance splineDistanceS = ss_spline_d[snappedSpline][1][snappedSplineLeadingPoint] + subsegmentDistance #Get offset distance - offsetY = ppd(qx,qy, snapped_x,snapped_y) + offsetY = utils.pointDistanceL2(qx,qy, snapped_x,snapped_y) if(mode): direction = getOrientation([splines[snappedSpline][snappedSplineLeadingPoint][0],splines[snappedSpline][snappedSplineLeadingPoint][1]] , [splines[snappedSpline][snappedSplineLeadingPoint+1][0],splines[snappedSpline][snappedSplineLeadingPoint+1][1]] , [qx,qy]) @@ -540,21 +526,38 @@ def similar(f1, f2, maxDistance2, maxDeltavelocity2): return (f1.position-f2.position).norm2Squared()<maxDistance2 and (f1.velocity-f2.velocity).norm2Squared()<maxDeltavelocity2 -def intersection(p1, p2, dp1, dp2): - '''Returns the intersection point between the two lines - defined by the respective vectors (dp) and origin points (p)''' - from numpy import matrix - from numpy.linalg import linalg - A = matrix([[dp1.y, -dp1.x], - [dp2.y, -dp2.x]]) - B = matrix([[dp1.y*p1.x-dp1.x*p1.y], - [dp2.y*p2.x-dp2.x*p2.y]]) - - if linalg.det(A) == 0: +def intersection(p1, p2, p3, p4): + ''' Intersection point (x,y) of lines formed by the vectors p1-p2 and p3-p4 + http://paulbourke.net/geometry/lineline2d/ + + If these lines are parralel, there will be a division by zero error. + ''' + dp12 = p2-p1 + det = ((p4.y-p3.y)*(p2.x-p1.x)-(p4.x-p3.x)*(p2.y-p1.y)) + if det == 0: return None else: - intersection = linalg.solve(A,B) - return Point(intersection[0,0], intersection[1,0]) + ua = ((p4.x-p3.x)*(p1.y-p3.y)-(p4.y-p3.y)*(p1.x-p3.x))/det + dp12.multiply(ua) + #x = p1.x + ua*(p2.x - p1.x) + #y = p1.y + ua*(p2.y - p1.y) + return p1+dp12 + +# def intersection(p1, p2, dp1, dp2): +# '''Returns the intersection point between the two lines +# defined by the respective vectors (dp) and origin points (p)''' +# from numpy import matrix +# from numpy.linalg import linalg +# A = matrix([[dp1.y, -dp1.x], +# [dp2.y, -dp2.x]]) +# B = matrix([[dp1.y*p1.x-dp1.x*p1.y], +# [dp2.y*p2.x-dp2.x*p2.y]]) + +# if linalg.det(A) == 0: +# return None +# else: +# intersection = linalg.solve(A,B) +# return Point(intersection[0,0], intersection[1,0]) def segmentIntersection(p1, p2, p3, p4): '''Returns the intersecting point of the segments [p1, p2] and [p3, p4], None otherwise''' @@ -562,9 +565,9 @@ 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()): return None else: - dp1 = p2-p1 - dp3 = p4-p3 - inter = intersection(p1, p3, dp1, dp3) + #dp1 = p2-p1 + #dp3 = p4-p3 + inter = intersection(p1, p2, p3, p4)#(p1, p3, dp1, dp3) if (inter != None and utils.inBetween(p1.x, p2.x, inter.x) and utils.inBetween(p3.x, p4.x, inter.x)
--- a/python/tests/moving.txt Tue Aug 05 17:45:36 2014 -0400 +++ b/python/tests/moving.txt Wed Aug 06 17:53:38 2014 -0400 @@ -59,11 +59,18 @@ >>> predictPositionNoLimit(10, Point(0,0), Point(1,1)) # doctest:+ELLIPSIS ((1.0...,1.0...), (10.0...,10.0...)) ->>> segmentIntersection(Point(0,0),Point(1,1), Point(0,1), Point(1,2)) ->>> segmentIntersection(Point(0,1),Point(1,0), Point(0,2), Point(2,1)) ->>> segmentIntersection(Point(0,0),Point(2,0), Point(1,-1),Point(1,1)) -(1.000000,0.000000) ->>> segmentIntersection(Point(0,1),Point(2,0),Point(1,1),Point(1,2)) +>>> segmentIntersection(Point(0,0), Point(0,1), Point(1,1), Point(2,3)) +>>> segmentIntersection(Point(0,1), Point(0,3), Point(1,0), Point(3,1)) +>>> segmentIntersection(Point(0,0), Point(2,2), Point(0,2), Point(2,0)) +(1.000000,1.000000) +>>> segmentIntersection(Point(0,1), Point(1,2), Point(2,0), Point(3,2)) + +>>> left = [(92.291666666666686, 102.99239033124439), (56.774193548387103, 69.688898836168306)] +>>> middle = [(87.211021505376351, 93.390778871978512), (59.032258064516128, 67.540286481647257)] +>>> right = [(118.82392473118281, 115.68263205013426), (63.172043010752688, 66.600268576544309)] +>>> alignments = [left, middle, right] +>>> getSYfromXY(73, 82, alignments) +[1, 0, 73.81997726720346, 81.10617000672484, 18.172277808821125, 18.172277808821125, 1.2129694042343868] >>> Trajectory().length() 0
--- a/python/utils.py Tue Aug 05 17:45:36 2014 -0400 +++ b/python/utils.py Wed Aug 06 17:53:38 2014 -0400 @@ -4,6 +4,7 @@ #from numpy import * #from pylab import * from datetime import time, datetime +from math import sqrt __metaclass__ = type @@ -241,7 +242,11 @@ return ceil(v*tens)/tens def inBetween(bound1, bound2, x): - return bound1 <= x <= bound2 or bound2 <= x<= bound1 + return bound1 <= x <= bound2 or bound2 <= x <= bound1 + +def pointDistanceL2(x1,y1,x2,y2): + ''' Compute point-to-point distance (L2 norm, ie Euclidean distance)''' + return sqrt((x2-x1)**2+(y2-y1)**2) def crossProduct(l1, l2): return l1[0]*l2[1]-l1[1]*l2[0]