tvaLib
tools_geo.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 # tvaLib Copyright (c) 2012-2016 Paul G. St-Aubin
3 # Ecole Polytechnique de Montreal, McGill University
4 # Python 2.7; (dt) Spyder Windows 10 64-bit; ipython Ubuntu 15.04 64-bit
5
9 import math as m
10
11 import numpy as np
12
13
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
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
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
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
70  addition to distance, by this much importance
71  (use zero to turn off)
72
73  Output:
74  =======
75  [spline index,
77  snapped x coordinate,
78  snapped y coordinate,
79  subsegment distance,
80  spline distance,
81  orthogonal point offset]
82  '''
84  else: [snappedSpline,snappedSplineLeadingPoint,snapped_x,snapped_y] = matchSplineNearest(qx, qy, splines)
85
87  try:
88  #Get sub-segment distance
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]
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
104  return [snappedSpline, snappedSplineLeadingPoint, snapped_x, snapped_y, subsegmentDistance, splineDistanceS, offsetY]
105  except:
106  return [None,None,None,None,None,None,None]
107
108
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
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)
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
159  snapped_x = splines[spline][spline_p][0]
160  snapped_y = splines[spline][spline_p][1]
161
163
164
165
167  ''' Identical to matchSplineNearest(), exept matching is based on distance
169
171  =================
172  vx,vy: heading vector comonents for target point q
174  addition to distance, by this much importance
175  (use zero to turn off)
176
178  For example, at a distance of 2.3m from a point, and a
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
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)
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
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
230  scores[spline] = temp_dists_min[spline] + (angles[spline]/180.0)**3*matchHeadingStrength
231  # and return best match
232  snappedSpline = scores.index(min(scores))
234
235
236
237
238
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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 subsec_spline_dist_incremental(spline)
Definition: tools_geo.py:388
def interpolateSplinePoints(positions, fraction)
Definition: tools_geo.py:412
def searchSplineProximity(splineA, splineB, minProximity=5.0, intersectionToEdgeMinFactor=2.0)
Definition: tools_geo.py:320
def vectorsToAngleDegCC(x1, y1, x2, y2)
Definition: tools_geo.py:44
def ppdSearchSquared(x1, y1, x2, y2, distanceSquared)
Definition: tools_geo.py:581
def interpolateVectorSeries(vectors, fraction)
Definition: tools_geo.py:433
def subsec_spline_dist_cumulative(spline)
Definition: tools_geo.py:400
def orthogonal_point_extension(distance, O, P1, PN1=None)
Definition: tools_geo.py:520
def getXYfromSY(s, spline, y=0)
Definition: tools_geo.py:239
def matchSplineNearest(qx, qy, splines)
Definition: tools_geo.py:109
def ppd(args)
Definition: tools_geo.py:566
def rotPointCC(x, y, angle, ox=0, oy=0)
The following functions are used for calculating geometry.
Definition: tools_geo.py:16
def searchSplineContinuousProximity(splineA, splineB, minProximity=5.0, minContinuousDistance=12)
Definition: tools_geo.py:357
def points_are_segmented(P1, P2, A, B)
Definition: tools_geo.py:560
def getSYfromXY(qx, qy, splines, matchHeadingStrength=10.0, vx=0.0, vy=0.0, orientation=False)
Definition: tools_geo.py:59
def pip(x, y, poly)
Definition: tools_geo.py:492
def ppldb2p(qx, qy, p0x, p0y, p1x, p1y)
Definition: tools_geo.py:590
def lineInter(P1, P2, P3, P4)
Definition: tools_geo.py:453