Mercurial Hosting > traffic-intelligence
changeset 573:cae4e5f3fe9f
fixed and simplified getSYfromXY
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Tue, 12 Aug 2014 17:47:16 -0400 |
parents | 9c429c7efe89 |
children | e24eeb244698 |
files | python/moving.py python/tests/moving.txt |
diffstat | 2 files changed, 37 insertions(+), 38 deletions(-) [+] |
line wrap: on
line diff
--- a/python/moving.py Tue Aug 12 15:33:03 2014 -0400 +++ b/python/moving.py Tue Aug 12 17:47:16 2014 -0400 @@ -179,8 +179,12 @@ def __neg__(self): return Point(-self.x, -self.y) + + def orthogonal(self): + return Point(self.y, -self.x) def multiply(self, alpha): + 'Warning, returns a new Point' return Point(self.x*alpha, self.y*alpha) def plot(self, options = 'o', **kwargs): @@ -244,6 +248,10 @@ return (counter%2 == 1); @staticmethod + def fromList(p): + return Point(p[0], p[1]) + + @staticmethod def dot(p1, p2): 'Scalar product' return p1.x*p2.x+p1.y*p2.y @@ -362,7 +370,7 @@ import pdb; pdb.set_trace() return X,Y -def getSYfromXY(qx, qy, splines, spline_assumption_threshold=0.5, mode=0): +def getSYfromXY(qx, qy, splines, goodEnoughSplineDistance = 0.5): ''' Snap a point (coordinates qx, qy) to it's nearest subsegment of it's nearest spline (from the list splines). To force snapping to a single spline, pass the spline alone through splines (e.g. splines=[splines[splineNum]]). @@ -381,48 +389,37 @@ #(buckle in, it gets ugly from here on out) ss_spline_d = subsec_spline_dist(splines) - temp_dist_min = float('inf') + minOffsetY = float('inf') #For each spline for spline in range(len(splines)): #For each spline point - for spline_p in range(len(splines[spline])): - if(spline_p > (len(splines[spline]) - 2)): - break - #Get point-intersection distance + for spline_p in range(len(splines[spline])-1): + #Get closest point on spline 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]) - if(X == False and Y == False): + if X == False and Y == False: 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(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 = utils.pointDistanceL2(qx,qy,X,Y) - if(temp_dist < temp_dist_min): - temp_dist_min = temp_dist - snappedSpline = spline - snappedSplineLeadingPoint = spline_p - snapped_x = X - snapped_y = Y - #Jump loop if significantly close - if(temp_dist < spline_assumption_threshold): break - - try: - #Get sub-segment distance - 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 = 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]) - if(direction == 'left'): offsetY = -offsetY - - return [snappedSpline, snappedSplineLeadingPoint, snapped_x, snapped_y, subsegmentDistance, splineDistanceS, offsetY] - except: - return [False,False,False,False,False,False,False] - + return None + 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): + offsetY = utils.pointDistanceL2(qx,qy,X,Y) + if offsetY < minOffsetY: + minOffsetY = offsetY + snappedSpline = spline + snappedSplineLeadingPoint = spline_p + snapped_x = X + snapped_y = Y + #Jump loop if significantly close + if offsetY < goodEnoughSplineDistance: + break + #Get sub-segment distance + 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 + orthogonalSplineVector = Point(splines[snappedSpline][snappedSplineLeadingPoint+1][1]-splines[snappedSpline][snappedSplineLeadingPoint][1], + splines[snappedSpline][snappedSplineLeadingPoint][0]-splines[snappedSpline][snappedSplineLeadingPoint+1][0])#positive orthogonal vector of vector (x,y) is (y, -x) + offsetVector = Point(qx-snapped_x, qy-snapped_y) + if Point.dot(orthogonalSplineVector, offsetVector) < 0: + minOffsetY = -minOffsetY + return [snappedSpline, snappedSplineLeadingPoint, snapped_x, snapped_y, subsegmentDistance, splineDistanceS, minOffsetY] def getXYfromSY(s, y, splineNum, splines, mode = 0): ''' Find X,Y coordinate from S,Y data.
--- a/python/tests/moving.txt Tue Aug 12 15:33:03 2014 -0400 +++ b/python/tests/moving.txt Tue Aug 12 17:47:16 2014 -0400 @@ -73,6 +73,8 @@ >>> alignments = [left, middle, right] >>> getSYfromXY(73, 82, alignments) [1, 0, 73.81997726720346, 81.10617000672484, 18.172277808821125, 18.172277808821125, 1.2129694042343868] +>>> getSYfromXY(78, 83, alignments, 0.5) +[1, 0, 77.03318826883, 84.05388936710071, 13.811799123113715, 13.811799123113715, -1.4301775140225983] >>> Trajectory().length() 0