Mercurial Hosting > traffic-intelligence
diff python/moving.py @ 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 |
line wrap: on
line diff
--- 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)