nsaunier/traffic-intelligence: python/moving.py comparison

comparison python/moving.py @ 573:cae4e5f3fe9f

fixed and simplified getSYfromXY

author	Nicolas Saunier <nicolas.saunier@polymtl.ca>
date	Tue, 12 Aug 2014 17:47:16 -0400
parents	a9c1d61a89b4
children	e24eeb244698

comparison

equal deleted inserted replaced

-:9c429c7efe89
+:cae4e5f3fe9f
 def __sub__(self, other):
 return Point(self.x-other.x, self.y-other.y)
 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):
 from matplotlib.pylab import plot
 plot([self.x], [self.y], options, **kwargs)
 xinters = (self.y-p1[1])*(p2[0]-p1[0])/(p2[1]-p1[1])+p1[0];
 if p1[0] == p2[0] or self.x <= xinters:
 counter+=1;
 p1=p2
 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
 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
-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]]).
 Output:
 '''
 #(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])):
+for spline_p in range(len(splines[spline])-1):
-if(spline_p > (len(splines[spline]) - 2)):
+#Get closest point on spline
-break
-#Get point-intersection distance
 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]
+return None
-#Check to see if point is not contained by subspline
+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):
-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
+offsetY = utils.pointDistanceL2(qx,qy,X,Y)
+if offsetY < minOffsetY:
-#Ok
+minOffsetY = offsetY
-temp_dist = utils.pointDistanceL2(qx,qy,X,Y)
+snappedSpline = spline
-if(temp_dist < temp_dist_min):
+snappedSplineLeadingPoint = spline_p
-temp_dist_min             = temp_dist
+snapped_x = X
-snappedSpline             = spline
+snapped_y = Y
-snappedSplineLeadingPoint = spline_p
+#Jump loop if significantly close
-snapped_x                 = X
+if offsetY < goodEnoughSplineDistance:
-snapped_y                 = Y
+break
-#Jump loop if significantly close
+#Get sub-segment distance
-if(temp_dist < spline_assumption_threshold): break
+subsegmentDistance = utils.pointDistanceL2(splines[snappedSpline][snappedSplineLeadingPoint][0],splines[snappedSpline][snappedSplineLeadingPoint][1],snapped_x,snapped_y)
+#Get total segment distance
-try:
+splineDistanceS = ss_spline_d[snappedSpline][1][snappedSplineLeadingPoint] + subsegmentDistance
-#Get sub-segment distance
+orthogonalSplineVector = Point(splines[snappedSpline][snappedSplineLeadingPoint+1][1]-splines[snappedSpline][snappedSplineLeadingPoint][1],
-subsegmentDistance = utils.pointDistanceL2(splines[snappedSpline][snappedSplineLeadingPoint][0],splines[snappedSpline][snappedSplineLeadingPoint][1],snapped_x,snapped_y)
+splines[snappedSpline][snappedSplineLeadingPoint][0]-splines[snappedSpline][snappedSplineLeadingPoint+1][0])#positive orthogonal vector of vector (x,y) is (y, -x)
-#Get total segment distance
+offsetVector = Point(qx-snapped_x, qy-snapped_y)
-splineDistanceS = ss_spline_d[snappedSpline][1][snappedSplineLeadingPoint] + subsegmentDistance
+if Point.dot(orthogonalSplineVector, offsetVector) < 0:
-#Get offset distance
+minOffsetY = -minOffsetY
-offsetY = utils.pointDistanceL2(qx,qy, snapped_x,snapped_y)
+return [snappedSpline, snappedSplineLeadingPoint, snapped_x, snapped_y, subsegmentDistance, splineDistanceS, minOffsetY]
-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]
 def getXYfromSY(s, y, splineNum, splines, mode = 0):
 ''' Find X,Y coordinate from S,Y data.
 if mode = 0 : return Snapped X,Y
 if mode !=0 : return Real X,Y

Mercurial > hg > nsaunier > traffic-intelligence

comparison python/moving.py @ 573:cae4e5f3fe9f