# HG changeset patch # User Nicolas Saunier # Date 1407362018 14400 # Node ID 0057c04f94d5bb852076565b13484f7326bc68e9 # Parent 072cedc3f33d81d841b901cfab6b1db591fee28f work in progress on intersections (for PET) diff -r 072cedc3f33d -r 0057c04f94d5 python/moving.py --- a/python/moving.py Tue Aug 05 17:45:36 2014 -0400 +++ b/python/moving.py Wed Aug 06 17:53:38 2014 -0400 @@ -309,20 +309,6 @@ # 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. @@ -347,7 +333,7 @@ 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]) @@ -390,7 +376,7 @@ subsegment distance, spline distance, orthogonal point offset] - ''' + ''' #(buckle in, it gets ugly from here on out) ss_spline_d = subsec_spline_dist(splines) @@ -408,10 +394,10 @@ 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 @@ -423,11 +409,11 @@ 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]) @@ -540,21 +526,38 @@ def similar(f1, f2, maxDistance2, maxDeltavelocity2): return (f1.position-f2.position).norm2Squared()>> predictPositionNoLimit(10, Point(0,0), Point(1,1)) # doctest:+ELLIPSIS ((1.0...,1.0...), (10.0...,10.0...)) ->>> segmentIntersection(Point(0,0),Point(1,1), Point(0,1), Point(1,2)) ->>> segmentIntersection(Point(0,1),Point(1,0), Point(0,2), Point(2,1)) ->>> segmentIntersection(Point(0,0),Point(2,0), Point(1,-1),Point(1,1)) -(1.000000,0.000000) ->>> segmentIntersection(Point(0,1),Point(2,0),Point(1,1),Point(1,2)) +>>> segmentIntersection(Point(0,0), Point(0,1), Point(1,1), Point(2,3)) +>>> segmentIntersection(Point(0,1), Point(0,3), Point(1,0), Point(3,1)) +>>> segmentIntersection(Point(0,0), Point(2,2), Point(0,2), Point(2,0)) +(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)] +>>> alignments = [left, middle, right] +>>> getSYfromXY(73, 82, alignments) +[1, 0, 73.81997726720346, 81.10617000672484, 18.172277808821125, 18.172277808821125, 1.2129694042343868] >>> Trajectory().length() 0 diff -r 072cedc3f33d -r 0057c04f94d5 python/utils.py --- a/python/utils.py Tue Aug 05 17:45:36 2014 -0400 +++ b/python/utils.py Wed Aug 06 17:53:38 2014 -0400 @@ -4,6 +4,7 @@ #from numpy import * #from pylab import * from datetime import time, datetime +from math import sqrt __metaclass__ = type @@ -241,7 +242,11 @@ return ceil(v*tens)/tens def inBetween(bound1, bound2, x): - return bound1 <= x <= bound2 or bound2 <= x<= bound1 + return bound1 <= x <= bound2 or bound2 <= x <= bound1 + +def pointDistanceL2(x1,y1,x2,y2): + ''' Compute point-to-point distance (L2 norm, ie Euclidean distance)''' + return sqrt((x2-x1)**2+(y2-y1)**2) def crossProduct(l1, l2): return l1[0]*l2[1]-l1[1]*l2[0]