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

comparison python/moving.py @ 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

comparison

equal deleted inserted replaced

-:072cedc3f33d
+:0057c04f94d5
 prepared_polygon = prep(polygon)
 return filter(prepared_polygon.contains, points)
 # 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.
 Output:
 =======
 ss_spline_d[spline][1] = zeros(len(splines[spline])-1)  #Cumulative distance
 ss_spline_d[spline][2] = zeros(len(splines[spline]))  #Cumulative distance with trailing distance
 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])
 ss_spline_d[spline][2][-1] = ss_spline_d[spline][2][-2] + ss_spline_d[spline][0][-1]
 snapped x coordinate,
 snapped y coordinate,
 subsegment distance,
 spline distance,
 orthogonal point offset]
 '''
 #(buckle in, it gets ugly from here on out)
 ss_spline_d = subsec_spline_dist(splines)
 temp_dist_min = float('inf')
 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):
 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
 snappedSplineLeadingPoint = spline_p
 snapped_x                 = X
 #Jump loop if significantly close
 if(temp_dist < spline_assumption_threshold): break
 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])
 if(direction == 'left'): offsetY = -offsetY
 @staticmethod
 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):
+def intersection(p1, p2, p3, p4):
-'''Returns the intersection point between the two lines
+''' Intersection point (x,y) of lines formed by the vectors p1-p2 and p3-p4
-defined by the respective vectors (dp) and origin points (p)'''
+http://paulbourke.net/geometry/lineline2d/
-from numpy import matrix
-from numpy.linalg import linalg
+If these lines are parralel, there will be a division by zero error.
-A = matrix([[dp1.y, -dp1.x],
+'''
-[dp2.y, -dp2.x]])
+dp12 = p2-p1
-B = matrix([[dp1.y*p1.x-dp1.x*p1.y],
+det = ((p4.y-p3.y)*(p2.x-p1.x)-(p4.x-p3.x)*(p2.y-p1.y))
-[dp2.y*p2.x-dp2.x*p2.y]])
+if det == 0:
-if linalg.det(A) == 0:
 return None
 else:
-intersection = linalg.solve(A,B)
+ua = ((p4.x-p3.x)*(p1.y-p3.y)-(p4.y-p3.y)*(p1.x-p3.x))/det
-return Point(intersection[0,0], intersection[1,0])
+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'''
 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
+#dp1 = p2-p1
-dp3 = p4-p3
+#dp3 = p4-p3
-inter = intersection(p1, p3, dp1, dp3)
+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)
 and utils.inBetween(p1.y, p2.y, inter.y)
 and utils.inBetween(p3.y, p4.y, inter.y)):

Mercurial > hg > nsaunier > traffic-intelligence

comparison python/moving.py @ 569:0057c04f94d5