tvaLib
Functions
lib.tools_geo Namespace Reference

Functions

def rotPointCC (x, y, angle, ox=0, oy=0)
 The following functions are used for calculating geometry. More...
 
def rotateVector (angle, vect, inputRadians=False)
 
def vectorsToAngleDegCC (x1, y1, x2, y2)
 
def getSYfromXY (qx, qy, splines, matchHeadingStrength=10.0, vx=0.0, vy=0.0, orientation=False)
 
def matchSplineNearest (qx, qy, splines)
 
def matchSplineNearestHeading (qx, qy, splines, vx, vy, matchHeadingStrength=10.0)
 
def getXYfromSY (s, spline, y=0)
 
def getOrientation (P1, P2, P3)
 
def getNearestXYinSplineFromXY (qx, qy, splines, searchPoint=0, searchRadius=5)
 
def searchSplineProximity (splineA, splineB, minProximity=5.0, intersectionToEdgeMinFactor=2.0)
 
def searchSplineContinuousProximity (splineA, splineB, minProximity=5.0, minContinuousDistance=12)
 
def subsec_spline_dist_incremental (spline)
 
def subsec_spline_dist_cumulative (spline)
 
def interpolateSplinePoints (positions, fraction)
 
def interpolateVectorSeries (vectors, fraction)
 
def lineInter (P1, P2, P3, P4)
 
def polyInter (P1, P2, poly, intersectEdges=False)
 
def pip (x, y, poly)
 
def midpoint (P1, P2)
 
def orthogonal_point_extension (distance, O, P1, PN1=None)
 
def orthogonal_joint_midpoint (O, P1, P2)
 
def points_are_segmented (P1, P2, A, B)
 
def ppd (args)
 
def ppdSearchSquared (x1, y1, x2, y2, distanceSquared)
 
def ppldb2p (qx, qy, p0x, p0y, p1x, p1y)
 

Function Documentation

◆ getNearestXYinSplineFromXY()

def lib.tools_geo.getNearestXYinSplineFromXY (   qx,
  qy,
  splines,
  searchPoint = 0,
  searchRadius = 5 
)
From a point qXqY, get nearest point (in XY coordinates) contained
    *inside* a set of splines (connector). 

    searchPoint=0: search leading points
    searchPoint=-1: search trailing points
    
    searchRadius: Algorythm will return False if no distance less than this

Definition at line 297 of file tools_geo.py.

297 def getNearestXYinSplineFromXY(qx, qy, splines, searchPoint=0, searchRadius=5):
298  ''' From a point qXqY, get nearest point (in XY coordinates) contained
299  *inside* a set of splines (connector).
300 
301  searchPoint=0: search leading points
302  searchPoint=-1: search trailing points
303 
304  searchRadius: Algorythm will return False if no distance less than this
305  '''
306  candidate = [float('inf'), 0, 0, 0, 0]
307 
308  for splineNum in range(len(splines)):
309 
310  d = ppd(qx,qy, splines[splineNum][searchPoint][0],splines[splineNum][searchPoint][1])
311  if(abs(d) <= searchRadius and d < candidate[0]): candidate = [d, splines[splineNum][searchPoint][0], splines[splineNum][searchPoint][1], None, splineNum]
312 
313 
314  [spline, snapped_A_p,snapped_x,snapped_y,subs_d,seg_d,d] = getSYfromXY(qx, qy, [splines[splineNum]])
315  if(spline is not False and seg_d > 0 and abs(d) <= searchRadius and abs(d) < candidate[0]): candidate = [abs(d), snapped_x, snapped_y, seg_d, splineNum]
316 
317  if(candidate[0] < searchRadius): return candidate
318  else: return False
319 
def ppd(args)
Definition: tools_geo.py:566
def getSYfromXY(qx, qy, splines, matchHeadingStrength=10.0, vx=0.0, vy=0.0, orientation=False)
Definition: tools_geo.py:59
def getNearestXYinSplineFromXY(qx, qy, splines, searchPoint=0, searchRadius=5)
Definition: tools_geo.py:297

◆ getOrientation()

def lib.tools_geo.getOrientation (   P1,
  P2,
  P3 
)
Determines if the point P3 is to the right or left
of a vector a described by P1-->P2.

RIGHT HAND RULE: Right of forwards+++++++; Left of forwards-------)    

vectorial multiplication of both vectors gives us information about the orientation of the snapped vector
To follow the right hand rule, we must use: 
               if z is (-), P is to the right of the spline and direction_y is thus +++++
               if z is (+), P is to the left of the spline and direction_y is thus ----         

=============
Returns "right" or "left"    

Definition at line 268 of file tools_geo.py.

268 def getOrientation(P1, P2, P3):
269  ''' Determines if the point P3 is to the right or left
270  of a vector a described by P1-->P2.
271 
272  RIGHT HAND RULE: Right of forwards+++++++; Left of forwards-------)
273 
274  vectorial multiplication of both vectors gives us information about the orientation of the snapped vector
275  To follow the right hand rule, we must use:
276  if z is (-), P is to the right of the spline and direction_y is thus +++++
277  if z is (+), P is to the left of the spline and direction_y is thus ----
278 
279  =============
280  Returns "right" or "left"
281 
282  '''
283 
284  vect_a_x = P2[0] - P1[0]
285  vect_a_y = P2[1] - P1[1]
286 
287  P4 = ppldb2p(P3[0],P3[1], P1[0],P1[1], P2[0],P2[1])
288 
289  vect_b_x = P3[0] - P4[0]
290  vect_b_y = P3[1] - P4[1]
291 
292  direction_z = (vect_a_x*vect_b_y - vect_b_x*vect_a_y)
293 
294  if direction_z < 0: return 'right'
295  else: return 'left'
296 
def ppldb2p(qx, qy, p0x, p0y, p1x, p1y)
Definition: tools_geo.py:590
def getOrientation(P1, P2, P3)
Definition: tools_geo.py:268

◆ getSYfromXY()

def lib.tools_geo.getSYfromXY (   qx,
  qy,
  splines,
  matchHeadingStrength = 10.0,
  vx = 0.0,
  vy = 0.0,
  orientation = False 
)
Find curvilinear coordinates
    
    Input:
    ======
    qx,qy:                coordinates of target point q
    splines:              list of splines
    orientation:          return offset with an orientation sign 
                          (<-^ is -, ^-> is +)
    vx,vy:                heading vector comonents for target point q
    matchHeadingStrength: use heading similarity to match trajectories, in
                          addition to distance, by this much importance
                          (use zero to turn off)
    
    Output:
    =======
    [spline index, 
    subsegment leading point index, 
    snapped x coordinate, 
    snapped y coordinate, 
    subsegment distance, 
    spline distance,
    orthogonal point offset]

Definition at line 59 of file tools_geo.py.

59 def getSYfromXY(qx, qy, splines, matchHeadingStrength=10.0, vx=0.0, vy=0.0, orientation=False):
60  ''' Find curvilinear coordinates
61 
62  Input:
63  ======
64  qx,qy: coordinates of target point q
65  splines: list of splines
66  orientation: return offset with an orientation sign
67  (<-^ is -, ^-> is +)
68  vx,vy: heading vector comonents for target point q
69  matchHeadingStrength: use heading similarity to match trajectories, in
70  addition to distance, by this much importance
71  (use zero to turn off)
72 
73  Output:
74  =======
75  [spline index,
76  subsegment leading point index,
77  snapped x coordinate,
78  snapped y coordinate,
79  subsegment distance,
80  spline distance,
81  orthogonal point offset]
82  '''
83  if(matchHeadingStrength): [snappedSpline,snappedSplineLeadingPoint,snapped_x,snapped_y] = matchSplineNearestHeading(qx, qy, splines, matchHeadingStrength=matchHeadingStrength, vx=vx, vy=vy)
84  else: [snappedSpline,snappedSplineLeadingPoint,snapped_x,snapped_y] = matchSplineNearest(qx, qy, splines)
85 
86  #Pull aditional curvilinear data
87  try:
88  #Get sub-segment distance
89  subsegmentDistance = ppd(splines[snappedSpline][snappedSplineLeadingPoint][0],splines[snappedSpline][snappedSplineLeadingPoint][1],snapped_x,snapped_y)
90  #Get offset distance
91  offsetY = ppd(qx,qy, snapped_x,snapped_y)
92 
93  ss_spline_c = subsec_spline_dist_cumulative(splines[snappedSpline])[:-1]
94  if(snappedSplineLeadingPoint >= len(ss_spline_c)):
95  #Get total segment distance
96  splineDistanceS = ss_spline_c[-1]
97  #Determine offset orientation
98  if(orientation and getOrientation([splines[snappedSpline][-2][0],splines[snappedSpline][-2][1]] , [splines[snappedSpline][-1][0],splines[snappedSpline][-1][1]] , [qx,qy]) == 'left'): offsetY = -offsetY
99  else:
100  #Get total segment distance
101  splineDistanceS = ss_spline_c[snappedSplineLeadingPoint] + subsegmentDistance
102  #Determine offset orientation
103  if(orientation and getOrientation([splines[snappedSpline][snappedSplineLeadingPoint][0],splines[snappedSpline][snappedSplineLeadingPoint][1]] , [splines[snappedSpline][snappedSplineLeadingPoint+1][0],splines[snappedSpline][snappedSplineLeadingPoint+1][1]] , [qx,qy]) == 'left'): offsetY = -offsetY
104  return [snappedSpline, snappedSplineLeadingPoint, snapped_x, snapped_y, subsegmentDistance, splineDistanceS, offsetY]
105  except:
106  return [None,None,None,None,None,None,None]
107 
108 
def subsec_spline_dist_cumulative(spline)
Definition: tools_geo.py:400
def matchSplineNearest(qx, qy, splines)
Definition: tools_geo.py:109
def ppd(args)
Definition: tools_geo.py:566
def getSYfromXY(qx, qy, splines, matchHeadingStrength=10.0, vx=0.0, vy=0.0, orientation=False)
Definition: tools_geo.py:59
def matchSplineNearestHeading(qx, qy, splines, vx, vy, matchHeadingStrength=10.0)
Definition: tools_geo.py:166
def getOrientation(P1, P2, P3)
Definition: tools_geo.py:268

◆ getXYfromSY()

def lib.tools_geo.getXYfromSY (   s,
  spline,
  y = 0 
)
Find X,Y coordinate from S,Y data on given spline. 

Definition at line 239 of file tools_geo.py.

239 def getXYfromSY(s, spline, y=0):
240  ''' Find X,Y coordinate from S,Y data on given spline. '''
241 
242 
243  ss_spline_c = subsec_spline_dist_cumulative(spline)
244 
245 
246  for sIx in range(1,len(spline)):
247  if(s < ss_spline_c[sIx]):
248  ss_dist = s - ss_spline_c[sIx-1]
249  #Get normal vector and then snap
250  vector_l_x = (spline[sIx][0] - spline[sIx-1][0])
251  vector_l_y = (spline[sIx][1] - spline[sIx-1][1])
252  magnitude = m.sqrt(vector_l_x**2 + vector_l_y**2)
253  n_vector_x = vector_l_x/magnitude
254  n_vector_y = vector_l_y/magnitude
255  snapped_x = spline[sIx-1][0] + ss_dist*n_vector_x
256  snapped_y = spline[sIx-1][1] + ss_dist*n_vector_y
257 
258  if(not y): return [snapped_x,snapped_y]
259  else:
260  #Real values (including orthogonal projection of y)
261  real_x = snapped_x - y*n_vector_y
262  real_y = snapped_y + y*n_vector_x
263  return [real_x,real_y]
264 
265  return [None,None]
266 
267 
def subsec_spline_dist_cumulative(spline)
Definition: tools_geo.py:400
def getXYfromSY(s, spline, y=0)
Definition: tools_geo.py:239

◆ interpolateSplinePoints()

def lib.tools_geo.interpolateSplinePoints (   positions,
  fraction 
)
For a series of positions [[x1,y1],...], reinterpolate position based
    on a fraction of the inter-position distances

Definition at line 412 of file tools_geo.py.

412 def interpolateSplinePoints(positions, fraction):
413  ''' For a series of positions [[x1,y1],...], reinterpolate position based
414  on a fraction of the inter-position distances
415  '''
416  if(len(positions) < 2): return positions
417  _positions = [positions[0]]
418  nextIx = 1
419  if(fraction < 1):
420  segment_completion = 0
421  while nextIx < len(positions):
422  if(1-segment_completion > fraction):
423  _positions.append((_positions[-1][0]+fraction*(positions[nextIx][0]-positions[nextIx-1][0]), _positions[-1][1]+fraction*(positions[nextIx][1]-positions[nextIx-1][1])))
424  segment_completion += fraction
425  else:
426  nextIx += 1
427  if(nextIx >= len(positions)): break
428  _positions.append((positions[nextIx][0]+(fraction-1+segment_completion)*(positions[nextIx][0]-positions[nextIx-1][0]), positions[nextIx][1]+(fraction-1+segment_completion)*(positions[nextIx][1]-positions[nextIx-1][1])))
429  segment_completion = fraction-1+segment_completion
430  return _positions
431 
432 
def interpolateSplinePoints(positions, fraction)
Definition: tools_geo.py:412

◆ interpolateVectorSeries()

def lib.tools_geo.interpolateVectorSeries (   vectors,
  fraction 
)
For a series of vectors [[x1,y1],...], reinterpolate vector magnitude
    based on a fraction of magnitude

Definition at line 433 of file tools_geo.py.

433 def interpolateVectorSeries(vectors, fraction):
434  ''' For a series of vectors [[x1,y1],...], reinterpolate vector magnitude
435  based on a fraction of magnitude
436  '''
437  _vectors = []
438  nextIx = 0
439  if(fraction < 1):
440  segment_completion = 0
441  while nextIx < len(vectors):
442  if(1-segment_completion > fraction):
443  _vectors.append((fraction*vectors[nextIx][0], fraction*vectors[nextIx][1]))
444  segment_completion += fraction
445  else:
446  nextIx += 1
447  if(nextIx >= len(vectors)): break
448  _vectors.append(((fraction-1+segment_completion)*vectors[nextIx][0]+(1-segment_completion)*vectors[nextIx-1][0], (fraction-1+segment_completion)*vectors[nextIx][1]+(1-segment_completion)*vectors[nextIx-1][0]))
449  segment_completion = fraction-1+segment_completion
450  return _vectors
451 
452 
def interpolateVectorSeries(vectors, fraction)
Definition: tools_geo.py:433

◆ lineInter()

def lib.tools_geo.lineInter (   P1,
  P2,
  P3,
  P4 
)
Intersection point (x,y) of lines formed by the vector P1-P2 and P3-P4
    https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection
    
    If these lines are parralel, there will be a division by zero error.
    
    NOTE: COORDINATES MUST BE PASSED AS FLOATS

Definition at line 453 of file tools_geo.py.

453 def lineInter(P1, P2, P3, P4):
454  ''' Intersection point (x,y) of lines formed by the vector P1-P2 and P3-P4
455  https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection
456 
457  If these lines are parralel, there will be a division by zero error.
458 
459  NOTE: COORDINATES MUST BE PASSED AS FLOATS
460  '''
461  pointList = [P1,P2,P3,P4]
462  for point in pointList:
463  if(pointList.count(point) >= 2): return point
464  ua = ((P4[0]-P3[0])*(P1[1]-P3[1])-(P4[1]-P3[1])*(P1[0]-P3[0]))/((P4[1]-P3[1])*(P2[0]-P1[0])-(P4[0]-P3[0])*(P2[1]-P1[1]))
465  x = P1[0] + ua*(P2[0] - P1[0])
466  y = P1[1] + ua*(P2[1] - P1[1])
467  return [x,y]
468 
469 
def lineInter(P1, P2, P3, P4)
Definition: tools_geo.py:453

◆ matchSplineNearest()

def lib.tools_geo.matchSplineNearest (   qx,
  qy,
  splines 
)
Snap a point (coordinates qx, qy) to its nearest subsegment of its
    nearest spline (from the list splines).
    
    To force snapping to a single spline, pass the spline alone through
    splines (e.g. splines=[splines[splineNum]]).
    
    Input:
    ======
    qx,qy:                coordinates of target point q
    splines:              list of splines
    
    Output:
    =======
    spline index
    
    Note: This function is ugly!

Definition at line 109 of file tools_geo.py.

109 def matchSplineNearest(qx, qy, splines):
110  ''' Snap a point (coordinates qx, qy) to its nearest subsegment of its
111  nearest spline (from the list splines).
112 
113  To force snapping to a single spline, pass the spline alone through
114  splines (e.g. splines=[splines[splineNum]]).
115 
116  Input:
117  ======
118  qx,qy: coordinates of target point q
119  splines: list of splines
120 
121  Output:
122  =======
123  spline index
124 
125  Note: This function is ugly!
126  '''
127 
128  temp_dist_min = float('inf')
129  #For each spline
130  for spline in range(len(splines)):
131  ss_splines_i = subsec_spline_dist_incremental(splines[spline])
132  found_subsegment=False
133  #For each leading spline point check containment on subspline
134  for spline_p in range(len(splines[spline])-1):
135  #Snap point to a line formed by spline segment vector
136  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])
137  if(X == None and Y == None):
138  print('Warning: Spline {0}, segment {1} has identical bounds and therefore is not a vector. Projection cannot continue.'.format(spline, spline_p))
139  continue
140  #Check to see if the snapped point is contained within the subspline
141  if(ss_splines_i[spline_p] < 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
142  found_subsegment=True
143  temp_dist = ppd(qx,qy,X,Y)
144  if(temp_dist < temp_dist_min):
145  temp_dist_min = temp_dist
146  snappedSpline = spline
147  snappedSplineLeadingPoint = spline_p
148  snapped_x = X
149  snapped_y = Y
150 
151  #Try again matching edges instead of segments (boundary condition between two subsegments with sharp angle)
152  if(not found_subsegment):
153  for spline_p in range(len(splines[spline])):
154  temp_dist = ppd(qx,qy,splines[spline][spline_p][0],splines[spline][spline_p][1])
155  if(temp_dist < temp_dist_min):
156  temp_dist_min = temp_dist
157  snappedSpline = spline
158  snappedSplineLeadingPoint = spline_p
159  snapped_x = splines[spline][spline_p][0]
160  snapped_y = splines[spline][spline_p][1]
161 
162  return [snappedSpline,snappedSplineLeadingPoint,snapped_x,snapped_y]
163 
164 
165 
def subsec_spline_dist_incremental(spline)
Definition: tools_geo.py:388
def matchSplineNearest(qx, qy, splines)
Definition: tools_geo.py:109
def ppd(args)
Definition: tools_geo.py:566
def ppldb2p(qx, qy, p0x, p0y, p1x, p1y)
Definition: tools_geo.py:590

◆ matchSplineNearestHeading()

def lib.tools_geo.matchSplineNearestHeading (   qx,
  qy,
  splines,
  vx,
  vy,
  matchHeadingStrength = 10.0 
)
Identical to matchSplineNearest(), exept matching is based on distance
    and heading
    
    Additional Input:
    =================
    vx,vy:                heading vector comonents for target point q
    matchHeadingStrength: use heading similarity to match trajectories, in
                          addition to distance, by this much importance
                          (use zero to turn off)
    
    Objective: min(distance + angleBetweenHeadings^2.5*matchHeadingStrength/100000)
               For example, at a distance of 2.3m from a point, and a
               heading difference of 136 and a matchHeadingStrength of 
               1.0, the equvalent dist by a heading mismatch is 2.15698
               and the effective distance score is thus 4.45m

    Note: This function is ugly!

Definition at line 166 of file tools_geo.py.

166 def matchSplineNearestHeading(qx, qy, splines, vx, vy, matchHeadingStrength=10.0):
167  ''' Identical to matchSplineNearest(), exept matching is based on distance
168  and heading
169 
170  Additional Input:
171  =================
172  vx,vy: heading vector comonents for target point q
173  matchHeadingStrength: use heading similarity to match trajectories, in
174  addition to distance, by this much importance
175  (use zero to turn off)
176 
177  Objective: min(distance + angleBetweenHeadings^2.5*matchHeadingStrength/100000)
178  For example, at a distance of 2.3m from a point, and a
179  heading difference of 136 and a matchHeadingStrength of
180  1.0, the equvalent dist by a heading mismatch is 2.15698
181  and the effective distance score is thus 4.45m
182 
183  Note: This function is ugly!
184  '''
185 
186  #Store minimum distance for each spline
187  temp_dists_min = [float('inf') for spline in range(len(splines))]
188  leadingPoints = [None for spline in range(len(splines))]
189  snapped_xs = [None for spline in range(len(splines))]
190  snapped_ys = [None for spline in range(len(splines))]
191  #For each spline, find nearest point
192  for spline in range(len(splines)):
193  ss_splines_i = subsec_spline_dist_incremental(splines[spline])
194  found_subsegment=False
195  #For each leading spline point check containment on subspline
196  for spline_p in range(len(splines[spline])-1):
197  #Snap point to a line formed by spline segment vector
198  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])
199  if(X == None and Y == None):
200  print('Warning: Spline {0}, segment {1} has identical bounds and therefore is not a vector. Projection cannot continue.'.format(spline, spline_p))
201  continue
202  #Check to see if the snapped point is contained within the subspline
203  if(ss_splines_i[spline_p] < 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
204  found_subsegment=True
205  temp_dist = ppd(qx,qy,X,Y)
206  if(temp_dist < temp_dists_min[spline]):
207  temp_dists_min[spline] = temp_dist
208  leadingPoints[spline] = spline_p
209  snapped_xs[spline] = X
210  snapped_ys[spline] = Y
211 
212  #Try again matching edges instead of segments (boundary condition between two subsegments with sharp angle)
213  if(not found_subsegment):
214  for spline_p in range(len(splines[spline])):
215  temp_dist = ppd(qx,qy,splines[spline][spline_p][0],splines[spline][spline_p][1])
216  if(temp_dist < temp_dists_min[spline]):
217  temp_dists_min[spline] = temp_dist
218  leadingPoints[spline] = spline_p
219  snapped_xs[spline] = splines[spline][spline_p][0]
220  snapped_ys[spline] = splines[spline][spline_p][1]
221 
222 
223  #Match heading and Score each spline match
224  angles = [None for spline in range(len(splines))]
225  scores = [float('inf') for spline in range(len(splines))]
226  for spline in range(len(splines)):
227  if(leadingPoints[spline] is None or len(splines) <= 1): continue
228  if(leadingPoints[spline] == len(splines[spline])-1): angles[spline] = vectorsToAngleDegCC(vx, vy, (splines[spline][leadingPoints[spline]][0]-splines[spline][leadingPoints[spline]-1][0]), (splines[spline][leadingPoints[spline]][1]-splines[spline][leadingPoints[spline]-1][1]))
229  else: angles[spline] = vectorsToAngleDegCC(vx, vy, (splines[spline][leadingPoints[spline]+1][0]-splines[spline][leadingPoints[spline]][0]), (splines[spline][leadingPoints[spline]+1][1]-splines[spline][leadingPoints[spline]][1]))
230  scores[spline] = temp_dists_min[spline] + (angles[spline]/180.0)**3*matchHeadingStrength
231  # and return best match
232  snappedSpline = scores.index(min(scores))
233  return [snappedSpline,leadingPoints[snappedSpline],snapped_xs[snappedSpline],snapped_ys[snappedSpline]]
234 
235 
236 
237 
238 
def subsec_spline_dist_incremental(spline)
Definition: tools_geo.py:388
def vectorsToAngleDegCC(x1, y1, x2, y2)
Definition: tools_geo.py:44
def ppd(args)
Definition: tools_geo.py:566
def ppldb2p(qx, qy, p0x, p0y, p1x, p1y)
Definition: tools_geo.py:590
def matchSplineNearestHeading(qx, qy, splines, vx, vy, matchHeadingStrength=10.0)
Definition: tools_geo.py:166

◆ midpoint()

def lib.tools_geo.midpoint (   P1,
  P2 
)
Get midpoint between two points. 

Definition at line 515 of file tools_geo.py.

515 def midpoint(P1,P2):
516  ''' Get midpoint between two points. '''
517  return [(P1[0]+P2[0])/2,(P1[1]+P2[1])/2]
518 
519 
def midpoint(P1, P2)
Definition: tools_geo.py:515

◆ orthogonal_joint_midpoint()

def lib.tools_geo.orthogonal_joint_midpoint (   O,
  P1,
  P2 
)
Return a vector. 

Definition at line 550 of file tools_geo.py.

550 def orthogonal_joint_midpoint(O, P1, P2):
551  ''' Return a vector. '''
552  #Check sides
553  if (pow(ppd(O[0],O[1],P1[0],P1[1]),2) + pow(ppd(O[0],O[1],P2[0],P2[1]),2)) > pow(ppd(P1[0],P1[1],P2[0],P2[1]),2):
554  return midpoint(P1,P2)
555  else:
556  print('Error: pva_geo_tools.orthogonal_joint_midpoint() needs work...')
557  return False
558 
559 
def ppd(args)
Definition: tools_geo.py:566
def orthogonal_joint_midpoint(O, P1, P2)
Definition: tools_geo.py:550
def midpoint(P1, P2)
Definition: tools_geo.py:515

◆ orthogonal_point_extension()

def lib.tools_geo.orthogonal_point_extension (   distance,
  O,
  P1,
  PN1 = None 
)
Return the coordinates of a point orthogonal to the line between O and
    P1 and also a given distance away from O. Right hand rule for
    distance sign (positive is to the right of the vector orientation). 
    
    If PN1 is defined, the average of the vectors PN1-O and O-P1 will be
    used. This acts as a sort of orthogonal midpoint if a point has
    different vectors on either side of a spline.
    
    To expand to 3 or more dimensions, additional points P2 or more, etc.
    are required, but this is not yet implemented. 

Definition at line 520 of file tools_geo.py.

520 def orthogonal_point_extension(distance, O, P1, PN1=None):
521  ''' Return the coordinates of a point orthogonal to the line between O and
522  P1 and also a given distance away from O. Right hand rule for
523  distance sign (positive is to the right of the vector orientation).
524 
525  If PN1 is defined, the average of the vectors PN1-O and O-P1 will be
526  used. This acts as a sort of orthogonal midpoint if a point has
527  different vectors on either side of a spline.
528 
529  To expand to 3 or more dimensions, additional points P2 or more, etc.
530  are required, but this is not yet implemented. '''
531  # Directional vector
532  vector = [P1[cix]-O[cix] for cix in range(len(O))]
533  mag = ppd(vector)
534  nvector = [vector[cix]/float(mag) for cix in range(len(O))]
535  # Directional vector 2 if available
536  if(PN1):
537  vector2 = [O[cix]-PN1[cix] for cix in range(len(O))]
538  mag = ppd(vector2)
539  nvector2 = [vector2[cix]/float(mag) for cix in range(len(O))]
540  #Average nvectors
541  nvector = [(nvector[cix] + nvector2[cix])/2.0 for cix in range(len(O))]
542  # Scale orthogonal vector to distance (only supports 2D at this time)
543  svector = [nvector[1]*distance, -nvector[0]*distance]
544  # Get point
545  return [O[cix]+svector[cix] for cix in range(len(O))]
546 
547 
548 
549 
def orthogonal_point_extension(distance, O, P1, PN1=None)
Definition: tools_geo.py:520
def ppd(args)
Definition: tools_geo.py:566

◆ pip()

def lib.tools_geo.pip (   x,
  y,
  poly 
)
Check if point is contained inside polygon.

    Input:
    ======
    x,y:  X and Y coordinates of point to check
    poly: A list of points

Definition at line 492 of file tools_geo.py.

492 def pip(x, y, poly):
493  ''' Check if point is contained inside polygon.
494 
495  Input:
496  ======
497  x,y: X and Y coordinates of point to check
498  poly: A list of points
499  '''
500  n = len(poly)
501  inside = False
502  p1x,p1y = poly[0][0],poly[0][1]
503  for i in range(n+1):
504  p2x,p2y = poly[i % n][0],poly[i % n][1]
505  if y > min(p1y,p2y):
506  if y <= max(p1y,p2y):
507  if x <= max(p1x,p2x):
508  if p1y != p2y:
509  xinters = (y-p1y)*(p2x-p1x)/(p2y-p1y)+p1x
510  if p1x == p2x or x <= xinters:
511  inside = not inside
512  p1x,p1y = p2x,p2y
513  return inside
514 
def pip(x, y, poly)
Definition: tools_geo.py:492

◆ points_are_segmented()

def lib.tools_geo.points_are_segmented (   P1,
  P2,
  A,
  B 
)
Check if points A and B are on opposite side of a vector between P1 and P2. 

Definition at line 560 of file tools_geo.py.

560 def points_are_segmented(P1, P2, A, B):
561  ''' Check if points A and B are on opposite side of a vector between P1 and P2. '''
562  if ((P1[1]-P2[1])*(A[0]-P1[0])+(P2[0]-P1[0])*(A[1]-P1[1]))*((P1[1]-P2[1])*(B[0]-P1[0])+(P2[0]-P1[0])*(B[1]-P1[1])) < 0: return True
563  else: return False
564 
565 
def points_are_segmented(P1, P2, A, B)
Definition: tools_geo.py:560

◆ polyInter()

def lib.tools_geo.polyInter (   P1,
  P2,
  poly,
  intersectEdges = False 
)
Returns a list of intersection points (x,y) of the vector P1-P2 and a
    series of vectors forming a polygon.
    
    Input:
    ======
    poly: A list of points
    
    NOTE: COORDINATES MUST BE PASSED AS FLOATS

Definition at line 470 of file tools_geo.py.

470 def polyInter(P1, P2, poly, intersectEdges=False):
471  ''' Returns a list of intersection points (x,y) of the vector P1-P2 and a
472  series of vectors forming a polygon.
473 
474  Input:
475  ======
476  poly: A list of points
477 
478  NOTE: COORDINATES MUST BE PASSED AS FLOATS
479  '''
480  intersections = []
481  if(poly[-1] != poly[0]): poly.append(poly[0])
482  for i in range(len(poly)-1):
483  try: P = lineInter(P1,P2,poly[i],poly[i+1])
484  except: continue
485  #The intersected point is real if the intersection to point distance is shorter than the segment length for ALL points
486  if(intersectEdges):
487  if(ppd(P,P1) <= ppd(P1,P2) and ppd(P,P2) <= ppd(P1,P2) and ppd(P,poly[i]) <= ppd(poly[i],poly[i+1]) and ppd(P,poly[i+1]) <= ppd(poly[i],poly[i+1])): intersections.append(P)
488  else:
489  if(ppd(P,P1) < ppd(P1,P2) and ppd(P,P2) < ppd(P1,P2) and ppd(P,poly[i]) < ppd(poly[i],poly[i+1]) and ppd(P,poly[i+1]) < ppd(poly[i],poly[i+1])): intersections.append(P)
490  return intersections
491 
def ppd(args)
Definition: tools_geo.py:566
def lineInter(P1, P2, P3, P4)
Definition: tools_geo.py:453
def polyInter(P1, P2, poly, intersectEdges=False)
Definition: tools_geo.py:470

◆ ppd()

def lib.tools_geo.ppd (   args)
Get point-to-point distance.
    
    Example usage:
    ==============
    d = ppd(x1,y1,x2,y2)    # X and Y coordinates
    d = ppd(P1,P2)          # Coordinates of two points
    d = ppd([V1,V2,...])    # Multi-dimensional vector

Definition at line 566 of file tools_geo.py.

566 def ppd(*args):
567  ''' Get point-to-point distance.
568 
569  Example usage:
570  ==============
571  d = ppd(x1,y1,x2,y2) # X and Y coordinates
572  d = ppd(P1,P2) # Coordinates of two points
573  d = ppd([V1,V2,...]) # Multi-dimensional vector
574  '''
575  if(len(args)==4): [x1,y1,x2,y2] = [args[0],args[1],args[2],args[3]]
576  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]]
577  elif(hasattr(args, '__iter__')): return m.pow(sum([m.pow(args[0][cix],2) for cix in range(len(args[0]))]), 1/float(len(args[0])))
578  else: raise Exception("Library error: ppd() in tools_geo takes exactly 2 or 4 arguments or a vector in the form of a list of coordinates.")
579  return m.sqrt(m.pow(x2-x1,2)+m.pow(y2-y1,2))
580 
def ppd(args)
Definition: tools_geo.py:566

◆ ppdSearchSquared()

def lib.tools_geo.ppdSearchSquared (   x1,
  y1,
  x2,
  y2,
  distanceSquared 
)
Verify that and return the point-to-point distance is inferior to
    distanceSquared. This method is more efficient for searches as m.sqrt is
    more expensive than m.pow.

Definition at line 581 of file tools_geo.py.

581 def ppdSearchSquared(x1,y1, x2,y2, distanceSquared):
582  ''' Verify that and return the point-to-point distance is inferior to
583  distanceSquared. This method is more efficient for searches as m.sqrt is
584  more expensive than m.pow.
585  '''
586  if((m.pow(x2-x1,2)+m.pow(y2-y1,2)) > distanceSquared): return False
587  else: return True
588 
589 
def ppdSearchSquared(x1, y1, x2, y2, distanceSquared)
Definition: tools_geo.py:581

◆ ppldb2p()

def lib.tools_geo.ppldb2p (   qx,
  qy,
  p0x,
  p0y,
  p1x,
  p1y 
)
Point-projection (Q) on line defined by 2 points (P0,P1). 
    http://cs.nyu.edu/~yap/classes/visual/03s/hw/h2/math.pdf

Definition at line 590 of file tools_geo.py.

590 def ppldb2p(qx,qy, p0x,p0y, p1x,p1y):
591  ''' Point-projection (Q) on line defined by 2 points (P0,P1).
592  http://cs.nyu.edu/~yap/classes/visual/03s/hw/h2/math.pdf
593  '''
594  if(p0x == p1x and p0y == p1y): return None,None
595  try:
596  #Approximate slope singularity by giving some slope roundoff; account for roundoff error
597  if(round(p0x, 10) == round(p1x, 10)):
598  p1x += 0.0000000001
599  if(round(p0y, 10) == round(p1y, 10)):
600  p1y += 0.0000000001
601  #make the calculation
602  Y = (-(qx)*(p0y-p1y)-(qy*m.pow((p0y-p1y),2))/(p0x-p1x)+m.pow(p0x,2)*(p0y-p1y)/(p0x-p1x)-p0x*p1x*(p0y-p1y)/(p0x-p1x)-p0y*(p0x-p1x))/(p1x-p0x-(m.pow(p0y-p1y,2)/(p0x-p1x)))
603  X = (-Y*(p1y-p0y)+qx*(p1x-p0x)+qy*(p1y-p0y))/(p1x-p0x)
604  except ZeroDivisionError:
605  print('Error: Division by zero in ppldb2p. Please report this error with the full traceback:')
606  print('qx={0}, qy={1}, p0x={2}, p0y={3}, p1x={4}, p1y={5}...'.format(qx, qy, p0x, p0y, p1x, p1y))
607  import pdb; pdb.set_trace()
608  return X,Y
609 
def ppldb2p(qx, qy, p0x, p0y, p1x, p1y)
Definition: tools_geo.py:590

◆ rotateVector()

def lib.tools_geo.rotateVector (   angle,
  vect,
  inputRadians = False 
)
Rotate a vector in Cartesian space by angle 
    http://en.wikipedia.org/wiki/Rotation_(mathematics)#Two_dimensions

Definition at line 31 of file tools_geo.py.

31 def rotateVector(angle, vect, inputRadians=False):
32  ''' Rotate a vector in Cartesian space by angle
33  http://en.wikipedia.org/wiki/Rotation_(mathematics)#Two_dimensions
34  '''
35 
36  if(not inputRadians): angle = angle*m.pi/180.0
37 
38  rotated_vector = [0,0]
39  rotated_vector[0] = m.cos(angle)*vect[0]-m.sin(angle)*vect[1]
40  rotated_vector[1] = m.sin(angle)*vect[0]+m.cos(angle)*vect[1]
41  return rotated_vector
42 
43 
def rotateVector(angle, vect, inputRadians=False)
Definition: tools_geo.py:31

◆ rotPointCC()

def lib.tools_geo.rotPointCC (   x,
  y,
  angle,
  ox = 0,
  oy = 0 
)

The following functions are used for calculating geometry.

Rotate 2D point counterclockwise about another 2D point (in degrees)

    Set frame of reference to optional [ox,oy]

Definition at line 16 of file tools_geo.py.

16 def rotPointCC(x, y, angle, ox=0, oy=0):
17  ''' Rotate 2D point counterclockwise about another 2D point (in degrees)
18 
19  Set frame of reference to optional [ox,oy]
20  '''
21  x = x - ox
22  y = y - oy
23  #Rotation matrix algebra
24  rx = float(x)*m.cos(float(angle)*m.pi/180) - float(y)*m.sin(float(angle)*m.pi/180)
25  ry = float(x)*m.sin(float(angle)*m.pi/180) + float(y)*m.cos(float(angle)*m.pi/180)
26  #Reset point to original coordinate
27  rx = rx + ox
28  ry = ry + oy
29  return rx, ry
30 
def rotPointCC(x, y, angle, ox=0, oy=0)
The following functions are used for calculating geometry.
Definition: tools_geo.py:16

◆ searchSplineContinuousProximity()

def lib.tools_geo.searchSplineContinuousProximity (   splineA,
  splineB,
  minProximity = 5.0,
  minContinuousDistance = 12 
)
For two splines, search for segment-segment proximity inferior to
    minProximity over a continuous distance.

Definition at line 357 of file tools_geo.py.

357 def searchSplineContinuousProximity(splineA, splineB, minProximity=5.0, minContinuousDistance=12):
358  ''' For two splines, search for segment-segment proximity inferior to
359  minProximity over a continuous distance.
360  '''
361 
362  indexPointsA = []
363  snappedAonB = []
364  indexPointsB = []
365  snappedBonA = []
366  for spline_p in range(len(splineA)):
367  [_, _,snapped_x,snapped_y,_,_,d] = getSYfromXY(splineA[spline_p][0], splineA[spline_p][1], [splineB])
368  if(d is not False and d > 0 and d < minProximity):
369  indexPointsA.append(spline_p)
370  snappedAonB.append([snapped_x,snapped_y])
371 
372  for spline_p in range(len(splineB)):
373  [_, _,snapped_x,snapped_y,_,_,d] = getSYfromXY(splineB[spline_p][0], splineB[spline_p][1], [splineA])
374  if(d is not False and d > 0 and d < minProximity):
375  indexPointsB.append(spline_p)
376  snappedBonA.append([snapped_x,snapped_y])
377 
378  if(len(indexPointsA) >= 2):
379  ss_spline_c = subsec_spline_dist_cumulative(splineA)
380  if((ss_spline_c[indexPointsA[-1]]-ss_spline_c[indexPointsA[0]]) > minContinuousDistance): return True
381  if(len(indexPointsB) >= 2):
382  ss_spline_c = subsec_spline_dist_cumulative(splineB)
383  if((ss_spline_c[indexPointsB[-1]]-ss_spline_c[indexPointsB[0]]) > minContinuousDistance): return True
384 
385  return False
386 
387 
def subsec_spline_dist_cumulative(spline)
Definition: tools_geo.py:400
def searchSplineContinuousProximity(splineA, splineB, minProximity=5.0, minContinuousDistance=12)
Definition: tools_geo.py:357
def getSYfromXY(qx, qy, splines, matchHeadingStrength=10.0, vx=0.0, vy=0.0, orientation=False)
Definition: tools_geo.py:59

◆ searchSplineProximity()

def lib.tools_geo.searchSplineProximity (   splineA,
  splineB,
  minProximity = 5.0,
  intersectionToEdgeMinFactor = 2.0 
)
For two splines, search for segment-segment proximity inferior to
    minProximity (do not check ends for non-intersection). Crossing points
    must have an intersection distance intersectionToEdgeMinFactor times
    larger than distances to edge or 1 times the minProximity to avoid
    edge detection overlap.

    TODO: check segment intersection 

Definition at line 320 of file tools_geo.py.

320 def searchSplineProximity(splineA, splineB, minProximity=5.0, intersectionToEdgeMinFactor=2.0):
321  ''' For two splines, search for segment-segment proximity inferior to
322  minProximity (do not check ends for non-intersection). Crossing points
323  must have an intersection distance intersectionToEdgeMinFactor times
324  larger than distances to edge or 1 times the minProximity to avoid
325  edge detection overlap.
326 
327  TODO: check segment intersection
328  '''
329 
330  dA = float('inf')
331  for spline_p in range(len(splineA)):
332  if(spline_p in [0, len(splineA)-1]): continue
333  [_, _,snapped_xA_,snapped_yA_,_,SA_,dA_] = getSYfromXY(splineA[spline_p][0], splineA[spline_p][1], [splineB])
334  if(dA_ == None): continue
335  # Edge overlap condition
336  if(ppd(splineA[spline_p][0],splineA[spline_p][1],splineA[0][0],splineA[0][1]) < max(minProximity,intersectionToEdgeMinFactor*dA_) or ppd(splineA[spline_p][0],splineA[spline_p][1],splineA[-1][0],splineA[-1][1]) < max(minProximity,intersectionToEdgeMinFactor*dA_)): continue
337  if(snapped_xA_ is not False and dA_ > 0 and dA_ < dA and dA_ < minProximity): [snapped_xA,snapped_yA,SA,dA,spline_pA] = [snapped_xA_,snapped_yA_,SA_,dA_,spline_p]
338  if(dA >= float('inf')): return False
339 
340  dB = float('inf')
341  for spline_p in range(len(splineB)):
342  if(spline_p in [0, len(splineB)-1]): continue
343  [_, _,snapped_xB_,snapped_yB_,_,SB_,dB_] = getSYfromXY(splineB[spline_p][0], splineB[spline_p][1], [splineA])
344  if(dB_ == None): continue
345  # Edge overlap condition
346  if(ppd(splineB[spline_p][0],splineB[spline_p][1],splineB[0][0],splineB[0][1]) < max(minProximity,intersectionToEdgeMinFactor*dB_) or ppd(splineB[spline_p][0],splineB[spline_p][1],splineB[-1][0],splineB[-1][1]) < max(minProximity,intersectionToEdgeMinFactor*dB_)): continue
347  if(snapped_xB_ is not False and dB_ > 0 and dB_ < dB and dB_ < minProximity): [snapped_xB,snapped_yB,SB,dB,spline_pB] = [snapped_xB_,snapped_yB_,SB_,dB_,spline_p]
348  if(dB >= float('inf')): return False
349 
350  ss_spline_cA = subsec_spline_dist_cumulative(splineA)
351  ss_spline_cB = subsec_spline_dist_cumulative(splineB)
352 
353  if(dA <= dB and dA < float('inf')): return [dA, splineA[spline_pA][0], splineA[spline_pA][1], ss_spline_cA[spline_pA], snapped_xA, snapped_yA, SA]
354  elif(dB <= dA and dB < float('inf')): return [dB, snapped_xB, snapped_yB, SB, splineB[spline_pB][0], splineB[spline_pB][1], ss_spline_cB[spline_pB]]
355  else: return False
356 
def searchSplineProximity(splineA, splineB, minProximity=5.0, intersectionToEdgeMinFactor=2.0)
Definition: tools_geo.py:320
def subsec_spline_dist_cumulative(spline)
Definition: tools_geo.py:400
def ppd(args)
Definition: tools_geo.py:566
def getSYfromXY(qx, qy, splines, matchHeadingStrength=10.0, vx=0.0, vy=0.0, orientation=False)
Definition: tools_geo.py:59

◆ subsec_spline_dist_cumulative()

def lib.tools_geo.subsec_spline_dist_cumulative (   spline)
Prepare a cumulative list of spline length subsegments from a 
    point-based spline, i.e. [[x1,y1],...]. The list of cumulative 
    distinces has the same size as the original spline, where each value is
    cumulative up to that point (thus the first point is always zero).

Definition at line 400 of file tools_geo.py.

401  ''' Prepare a cumulative list of spline length subsegments from a
402  point-based spline, i.e. [[x1,y1],...]. The list of cumulative
403  distinces has the same size as the original spline, where each value is
404  cumulative up to that point (thus the first point is always zero).
405  '''
406 
407  #Prepare subsegment increments
408  ss_spline_i = subsec_spline_dist_incremental(spline)
409  return [sum(ss_spline_i[:i]) for i in range(len(spline))]
410 
411 
def subsec_spline_dist_incremental(spline)
Definition: tools_geo.py:388
def subsec_spline_dist_cumulative(spline)
Definition: tools_geo.py:400

◆ subsec_spline_dist_incremental()

def lib.tools_geo.subsec_spline_dist_incremental (   spline)
Prepare an incremental list of spline length subsegments from a 
    point-based spline, i.e. [[x1,y1],...]. The list of increments is one
    less than the original spline.

Definition at line 388 of file tools_geo.py.

389  ''' Prepare an incremental list of spline length subsegments from a
390  point-based spline, i.e. [[x1,y1],...]. The list of increments is one
391  less than the original spline.
392  '''
393 
394  ss_spline_i = []
395  for spIx in range(1,len(spline)):
396  ss_spline_i.append(ppd(spline[spIx-1][0],spline[spIx-1][1],spline[spIx][0],spline[spIx][1]))
397  return ss_spline_i
398 
399 
def subsec_spline_dist_incremental(spline)
Definition: tools_geo.py:388
def ppd(args)
Definition: tools_geo.py:566

◆ vectorsToAngleDegCC()

def lib.tools_geo.vectorsToAngleDegCC (   x1,
  y1,
  x2,
  y2 
)
Determine counterclockwise angle between 2 2D vectors (in degrees).
    
    where:
        x1 and y1 are compoents of vector 1
        x2 and y2 are compoents of vector 2

Definition at line 44 of file tools_geo.py.

44 def vectorsToAngleDegCC(x1, y1, x2, y2):
45  ''' Determine counterclockwise angle between 2 2D vectors (in degrees).
46 
47  where:
48  x1 and y1 are compoents of vector 1
49  x2 and y2 are compoents of vector 2
50  '''
51 
52  div = (m.sqrt(m.pow(float(x1),2)+m.pow(float(y1),2))*m.sqrt(m.pow(float(x2),2)+m.pow(float(y2),2)))
53  if(div == 0): return 0.0
54  cosine = (float(x1)*float(x2)+float(y1)*float(y2))/div
55  if(cosine > 1 or cosine < -1): return 0.0
56  else: return m.acos(cosine)*180/m.pi
57 
58 
def vectorsToAngleDegCC(x1, y1, x2, y2)
Definition: tools_geo.py:44