Mercurial Hosting > traffic-intelligence
changeset 577:d0abd2ee17b9
changed arguments to type Point
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Thu, 28 Aug 2014 17:22:38 -0400 |
parents | 0eff0471f9cb |
children | fe4e9d2b807d |
files | python/moving.py python/tests/moving.txt |
diffstat | 2 files changed, 45 insertions(+), 41 deletions(-) [+] |
line wrap: on
line diff
diff -r 0eff0471f9cb -r d0abd2ee17b9 python/moving.py --- a/python/moving.py Thu Aug 28 16:42:13 2014 -0400 +++ b/python/moving.py Thu Aug 28 17:22:38 2014 -0400 @@ -362,7 +362,7 @@ http://cs.nyu.edu/~yap/classes/visual/03s/hw/h2/math.pdf ''' if(p0x == p1x and p0y == p1y): - return False,False + return None try: #Approximate slope singularity by giving some slope roundoff; account for roundoff error if(round(p0x, 10) == round(p1x, 10)): @@ -376,10 +376,10 @@ print('Error: Division by zero in ppldb2p. Please report this error with the full traceback:') print('qx={0}, qy={1}, p0x={2}, p0y={3}, p1x={4}, p1y={5}...'.format(qx, qy, p0x, p0y, p1x, p1y)) import pdb; pdb.set_trace() - return X,Y + return Point(X,Y) -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). +def getSYfromXY(p, splines, goodEnoughSplineDistance = 0.5): + ''' Snap a point p 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]]). @@ -393,42 +393,39 @@ spline distance, orthogonal point offset] ''' - - #(buckle in, it gets ugly from here on out) - ss_spline_d = subsec_spline_dist(splines) - minOffsetY = float('inf') #For each spline for spline in range(len(splines)): - #For each spline point + #For each spline point index 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: + closestPoint = ppldb2p(p.x,p.y,splines[spline][spline_p][0],splines[spline][spline_p][1],splines[spline][spline_p+1][0],splines[spline][spline_p+1][1]) + if closestPoint == None: print('Error: Spline {0}, segment {1} has identical bounds and therefore is not a vector. Projection cannot continue.'.format(spline, spline_p)) 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 utils.inBetween(splines[spline][spline_p][0], splines[spline][spline_p+1][0], p.x) and utils.inBetween(splines[spline][spline_p][1], splines[spline][spline_p+1][1], p.y): + offsetY = Point.distanceNorm2(closestPoint, p)#utils.pointDistanceL2(p.x,p.y,X,Y) if offsetY < minOffsetY: minOffsetY = offsetY snappedSpline = spline snappedSplineLeadingPoint = spline_p - snapped_x = X - snapped_y = Y + snappedPoint = Point(closestPoint.x, closestPoint.y) #Jump loop if significantly close if offsetY < goodEnoughSplineDistance: break #Get sub-segment distance if minOffsetY != float('inf'): - 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) + subsegmentDistance = Point.distanceNorm2(snappedPoint, splines[snappedSpline][snappedSplineLeadingPoint]) + #subsegmentDistance = utils.pointDistanceL2(splines[snappedSpline][snappedSplineLeadingPoint][0],splines[snappedSpline][snappedSplineLeadingPoint][1],snapped_x,snapped_y) + #Get cumulative alignment distance (total segment distance) + splineDistanceS = splines[snappedSpline].getCumulativeDistance(snappedSplineLeadingPoint) + subsegmentDistance + orthogonalSplineVector = (splines[snappedSpline][snappedSplineLeadingPoint+1]-splines[snappedSpline][snappedSplineLeadingPoint]).orthogonal() +#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 = p-snappedPoint#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] + return [snappedSpline, snappedSplineLeadingPoint, snappedPoint, subsegmentDistance, splineDistanceS, minOffsetY] else: return None @@ -743,18 +740,18 @@ from numpy import hypot return hypot(self.positions[0], self.positions[1]) - def cumulatedDisplacement(self): - 'Returns the sum of the distances between each successive point' - displacement = 0 - for i in xrange(self.length()-1): - displacement += Point.distanceNorm2(self.__getitem__(i),self.__getitem__(i+1)) - return displacement + # def cumulatedDisplacement(self): + # 'Returns the sum of the distances between each successive point' + # displacement = 0 + # for i in xrange(self.length()-1): + # displacement += Point.distanceNorm2(self.__getitem__(i),self.__getitem__(i+1)) + # return displacement def computeCumulativeDistances(self): '''Computes the distance from each point to the next and the cumulative distance up to the point Can be accessed through getDistance(idx) and getCumulativeDistance(idx)''' self.distances = [] - self.cumulativeDistances = [] + self.cumulativeDistances = [0.] p1 = self[0] cumulativeDistance = 0. for i in xrange(self.length()-1): @@ -772,11 +769,11 @@ print('Index {} beyond trajectory length {}-1'.format(i, self.length())) def getCumulativeDistance(self, i): - '''Return the cumulative distance between the beginning and point i+1''' - if i < self.length()-1: + '''Return the cumulative distance between the beginning and point i''' + if i < self.length(): return self.cumulativeDistances[i] else: - print('Index {} beyond trajectory length {}-1'.format(i, self.length())) + print('Index {} beyond trajectory length {}'.format(i, self.length())) def similarOrientation(self, refDirection, cosineThreshold, minProportion = 0.5): '''Indicates whether the minProportion (<=1.) (eg half) of the trajectory elements (vectors for velocity)
diff -r 0eff0471f9cb -r d0abd2ee17b9 python/tests/moving.txt --- a/python/tests/moving.txt Thu Aug 28 16:42:13 2014 -0400 +++ b/python/tests/moving.txt Thu Aug 28 17:22:38 2014 -0400 @@ -67,14 +67,15 @@ (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)] +>>> left = Trajectory.fromPointList([(92.291666666666686, 102.99239033124439), (56.774193548387103, 69.688898836168306)]) +>>> middle = Trajectory.fromPointList([(87.211021505376351, 93.390778871978512), (59.032258064516128, 67.540286481647257)]) +>>> right = Trajectory.fromPointList([(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] ->>> getSYfromXY(78, 83, alignments, 0.5) -[1, 0, 77.03318826883, 84.05388936710071, 13.811799123113715, 13.811799123113715, -1.4301775140225983] +>>> for a in alignments: a.computeCumulativeDistances() +>>> getSYfromXY(Point(73, 82), alignments) +[1, 0, (73.819977,81.106170), 18.172277808821125, 18.172277808821125, 1.2129694042343868] +>>> getSYfromXY(Point(78, 83), alignments, 0.5) +[1, 0, (77.033188,84.053889), 13.811799123113715, 13.811799123113715, -1.4301775140225983] >>> Trajectory().length() 0 @@ -101,10 +102,16 @@ 2.23606797749979 >>> t1.getDistance(1) 5.385164807134504 +>>> t1.getDistance(2) +Index 2 beyond trajectory length 3-1 +>>> t1.getCumulativeDistance(0) +0.0 >>> t1.getCumulativeDistance(1) +2.23606797749979 +>>> t1.getCumulativeDistance(2) 7.6212327846342935 ->>> t1.getCumulativeDistance(2) -Index 2 beyond trajectory length 3-1 +>>> t1.getCumulativeDistance(3) +Index 3 beyond trajectory length 3 >>> from utils import LCSS