tools_geo.py

Go to the documentation of this file.

#!/usr/bin/env python
#  tvaLib Copyright (c) 2012-2016 Paul G. St-Aubin
#  Ecole Polytechnique de Montreal, McGill University
#  Python 2.7; (dt) Spyder Windows 10 64-bit; ipython Ubuntu 15.04 64-bit

import math  as m

import numpy as np
    

def rotPointCC(x, y, angle, ox=0, oy=0):
    ''' Rotate 2D point counterclockwise about another 2D point (in degrees)
    
        Set frame of reference to optional [ox,oy]
        '''
    x = x - ox
    y = y - oy
    #Rotation matrix algebra
    rx = float(x)*m.cos(float(angle)*m.pi/180) - float(y)*m.sin(float(angle)*m.pi/180)
    ry = float(x)*m.sin(float(angle)*m.pi/180) + float(y)*m.cos(float(angle)*m.pi/180)
    #Reset point to original coordinate
    rx = rx + ox
    ry = ry + oy
    return rx, ry
    
def rotateVector(angle, vect, inputRadians=False):
    ''' Rotate a vector in Cartesian space by angle 
        http://en.wikipedia.org/wiki/Rotation_(mathematics)#Two_dimensions
        '''
    
    if(not inputRadians): angle = angle*m.pi/180.0
    
    rotated_vector = [0,0]
    rotated_vector[0] = m.cos(angle)*vect[0]-m.sin(angle)*vect[1]
    rotated_vector[1] = m.sin(angle)*vect[0]+m.cos(angle)*vect[1]
    return rotated_vector


def 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
        '''
    
    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)))
    if(div == 0): return 0.0
    cosine = (float(x1)*float(x2)+float(y1)*float(y2))/div
    if(cosine > 1 or cosine < -1): return 0.0
    else:                          return m.acos(cosine)*180/m.pi


def 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]
        '''
    if(matchHeadingStrength): [snappedSpline,snappedSplineLeadingPoint,snapped_x,snapped_y] = matchSplineNearestHeading(qx, qy, splines, matchHeadingStrength=matchHeadingStrength, vx=vx, vy=vy)
    else:                     [snappedSpline,snappedSplineLeadingPoint,snapped_x,snapped_y] = matchSplineNearest(qx, qy, splines)
    
    #Pull aditional curvilinear data
    try:
        #Get sub-segment distance
        subsegmentDistance = ppd(splines[snappedSpline][snappedSplineLeadingPoint][0],splines[snappedSpline][snappedSplineLeadingPoint][1],snapped_x,snapped_y)
        #Get offset distance    
        offsetY = ppd(qx,qy, snapped_x,snapped_y)
        
        ss_spline_c = subsec_spline_dist_cumulative(splines[snappedSpline])[:-1]          
        if(snappedSplineLeadingPoint >= len(ss_spline_c)):
            #Get total segment distance
            splineDistanceS = ss_spline_c[-1]
            #Determine offset orientation
            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
        else:
            #Get total segment distance
            splineDistanceS = ss_spline_c[snappedSplineLeadingPoint] + subsegmentDistance
            #Determine offset orientation
            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
        return [snappedSpline, snappedSplineLeadingPoint, snapped_x, snapped_y, subsegmentDistance, splineDistanceS, offsetY]
    except:
        return [None,None,None,None,None,None,None]
    

def 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!
        '''

    temp_dist_min = float('inf')
    #For each spline
    for spline in range(len(splines)):
        ss_splines_i = subsec_spline_dist_incremental(splines[spline])
        found_subsegment=False
        #For each leading spline point check containment on subspline
        for spline_p in range(len(splines[spline])-1):
            #Snap point to a line formed by spline segment vector
            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 == None and Y == None):
                print('Warning: Spline {0}, segment {1} has identical bounds and therefore is not a vector. Projection cannot continue.'.format(spline, spline_p))
                continue
            #Check to see if the snapped point is contained within the subspline
            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          
            found_subsegment=True
            temp_dist = ppd(qx,qy,X,Y)
            if(temp_dist < temp_dist_min):
                temp_dist_min             = temp_dist
                snappedSpline             = spline
                snappedSplineLeadingPoint = spline_p
                snapped_x                 = X
                snapped_y                 = Y
        
        #Try again matching edges instead of segments (boundary condition between two subsegments with sharp angle)
        if(not found_subsegment):
            for spline_p in range(len(splines[spline])):
                temp_dist = ppd(qx,qy,splines[spline][spline_p][0],splines[spline][spline_p][1])
                if(temp_dist < temp_dist_min):
                    temp_dist_min             = temp_dist
                    snappedSpline             = spline
                    snappedSplineLeadingPoint = spline_p
                    snapped_x                 = splines[spline][spline_p][0]
                    snapped_y                 = splines[spline][spline_p][1]     
    
    return [snappedSpline,snappedSplineLeadingPoint,snapped_x,snapped_y]
    


def 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!
        '''
    
    #Store minimum distance for each spline
    temp_dists_min = [float('inf') for spline in range(len(splines))]
    leadingPoints = [None for spline in range(len(splines))]
    snapped_xs = [None for spline in range(len(splines))]
    snapped_ys = [None for spline in range(len(splines))]
    #For each spline, find nearest point
    for spline in range(len(splines)):
        ss_splines_i = subsec_spline_dist_incremental(splines[spline])
        found_subsegment=False
        #For each leading spline point check containment on subspline
        for spline_p in range(len(splines[spline])-1):
            #Snap point to a line formed by spline segment vector
            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 == None and Y == None):
                print('Warning: Spline {0}, segment {1} has identical bounds and therefore is not a vector. Projection cannot continue.'.format(spline, spline_p))
                continue
            #Check to see if the snapped point is contained within the subspline
            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          
            found_subsegment=True
            temp_dist = ppd(qx,qy,X,Y)
            if(temp_dist < temp_dists_min[spline]):
                temp_dists_min[spline] = temp_dist
                leadingPoints[spline]  = spline_p
                snapped_xs[spline]     = X
                snapped_ys[spline]     = Y
        
        #Try again matching edges instead of segments (boundary condition between two subsegments with sharp angle)
        if(not found_subsegment):
            for spline_p in range(len(splines[spline])):
                temp_dist = ppd(qx,qy,splines[spline][spline_p][0],splines[spline][spline_p][1])
                if(temp_dist < temp_dists_min[spline]):
                    temp_dists_min[spline] = temp_dist
                    leadingPoints[spline]  = spline_p
                    snapped_xs[spline]     = splines[spline][spline_p][0]
                    snapped_ys[spline]     = splines[spline][spline_p][1]     
    
    
    #Match heading and Score each spline match
    angles = [None for spline in range(len(splines))]
    scores = [float('inf') for spline in range(len(splines))]
    for spline in range(len(splines)):
        if(leadingPoints[spline] is None or len(splines) <= 1): continue
        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]))
        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]))
        scores[spline] = temp_dists_min[spline] + (angles[spline]/180.0)**3*matchHeadingStrength
    # and return best match
    snappedSpline = scores.index(min(scores))
    return [snappedSpline,leadingPoints[snappedSpline],snapped_xs[snappedSpline],snapped_ys[snappedSpline]]
    




def getXYfromSY(s, spline, y=0):
    ''' Find X,Y coordinate from S,Y data on given spline. ''' 
    
    
    ss_spline_c = subsec_spline_dist_cumulative(spline)
    
    
    for sIx in range(1,len(spline)):
        if(s < ss_spline_c[sIx]):
            ss_dist = s - ss_spline_c[sIx-1]
            #Get normal vector and then snap
            vector_l_x = (spline[sIx][0] - spline[sIx-1][0])
            vector_l_y = (spline[sIx][1] - spline[sIx-1][1])
            magnitude  = m.sqrt(vector_l_x**2 + vector_l_y**2)
            n_vector_x = vector_l_x/magnitude
            n_vector_y = vector_l_y/magnitude
            snapped_x  = spline[sIx-1][0] + ss_dist*n_vector_x
            snapped_y  = spline[sIx-1][1] + ss_dist*n_vector_y

            if(not y): return [snapped_x,snapped_y]
            else:
                #Real values (including orthogonal projection of y)
                real_x = snapped_x - y*n_vector_y 
                real_y = snapped_y + y*n_vector_x            
                return [real_x,real_y]
    
    return [None,None]


def 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"    
    
    '''    
     
    vect_a_x = P2[0] - P1[0]       
    vect_a_y = P2[1] - P1[1]
    
    P4 = ppldb2p(P3[0],P3[1], P1[0],P1[1], P2[0],P2[1])        
        
    vect_b_x = P3[0] - P4[0]
    vect_b_y = P3[1] - P4[1]
            
    direction_z = (vect_a_x*vect_b_y - vect_b_x*vect_a_y)
    
    if direction_z < 0: return 'right'
    else:               return 'left'
    
def 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
        '''
    candidate   = [float('inf'), 0, 0, 0, 0]
    
    for splineNum in range(len(splines)):
        
        d = ppd(qx,qy, splines[splineNum][searchPoint][0],splines[splineNum][searchPoint][1])
        if(abs(d) <= searchRadius and d < candidate[0]): candidate = [d, splines[splineNum][searchPoint][0], splines[splineNum][searchPoint][1], None, splineNum]

        
        [spline, snapped_A_p,snapped_x,snapped_y,subs_d,seg_d,d] = getSYfromXY(qx, qy, [splines[splineNum]])
        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]

    if(candidate[0] < searchRadius): return candidate
    else:                            return False

def 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 
        '''
    
    dA = float('inf')
    for spline_p in range(len(splineA)):
        if(spline_p in [0, len(splineA)-1]): continue
        [_, _,snapped_xA_,snapped_yA_,_,SA_,dA_] = getSYfromXY(splineA[spline_p][0], splineA[spline_p][1], [splineB])
        if(dA_ == None): continue
        # Edge overlap condition
        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
        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]
    if(dA >= float('inf')): return False
 
    dB = float('inf')  
    for spline_p in range(len(splineB)):
        if(spline_p in [0, len(splineB)-1]): continue
        [_, _,snapped_xB_,snapped_yB_,_,SB_,dB_] = getSYfromXY(splineB[spline_p][0], splineB[spline_p][1], [splineA])
        if(dB_ == None): continue
        # Edge overlap condition
        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
        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]
    if(dB >= float('inf')): return False
    
    ss_spline_cA = subsec_spline_dist_cumulative(splineA)
    ss_spline_cB = subsec_spline_dist_cumulative(splineB)
    
    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]
    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]]
    else:                           return False

def searchSplineContinuousProximity(splineA, splineB, minProximity=5.0, minContinuousDistance=12):
    ''' For two splines, search for segment-segment proximity inferior to
        minProximity over a continuous distance.
        '''
        
    indexPointsA = []
    snappedAonB  = []
    indexPointsB = []
    snappedBonA  = []
    for spline_p in range(len(splineA)):
        [_, _,snapped_x,snapped_y,_,_,d] = getSYfromXY(splineA[spline_p][0], splineA[spline_p][1], [splineB])
        if(d is not False and d > 0 and d < minProximity): 
            indexPointsA.append(spline_p)
            snappedAonB.append([snapped_x,snapped_y])
    
    for spline_p in range(len(splineB)):
        [_, _,snapped_x,snapped_y,_,_,d] = getSYfromXY(splineB[spline_p][0], splineB[spline_p][1], [splineA])
        if(d is not False and d > 0 and d < minProximity): 
            indexPointsB.append(spline_p)
            snappedBonA.append([snapped_x,snapped_y])
            
    if(len(indexPointsA) >= 2):
        ss_spline_c = subsec_spline_dist_cumulative(splineA)
        if((ss_spline_c[indexPointsA[-1]]-ss_spline_c[indexPointsA[0]]) > minContinuousDistance):  return True
    if(len(indexPointsB) >= 2):
        ss_spline_c = subsec_spline_dist_cumulative(splineB)
        if((ss_spline_c[indexPointsB[-1]]-ss_spline_c[indexPointsB[0]]) > minContinuousDistance):  return True
    
    return False
    

def 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.
        '''
    
    ss_spline_i = []
    for spIx in range(1,len(spline)):
        ss_spline_i.append(ppd(spline[spIx-1][0],spline[spIx-1][1],spline[spIx][0],spline[spIx][1]))
    return ss_spline_i


def 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).
        '''
    
    #Prepare subsegment increments
    ss_spline_i = subsec_spline_dist_incremental(spline)
    return [sum(ss_spline_i[:i]) for i in range(len(spline))]


def interpolateSplinePoints(positions, fraction):
    ''' For a series of positions [[x1,y1],...], reinterpolate position based
        on a fraction of the inter-position distances
        '''
    if(len(positions) < 2): return positions
    _positions = [positions[0]]
    nextIx = 1
    if(fraction < 1):
        segment_completion = 0
        while nextIx < len(positions):
            if(1-segment_completion > fraction):
                _positions.append((_positions[-1][0]+fraction*(positions[nextIx][0]-positions[nextIx-1][0]), _positions[-1][1]+fraction*(positions[nextIx][1]-positions[nextIx-1][1])))
                segment_completion += fraction
            else:
                nextIx += 1
                if(nextIx >= len(positions)): break
                _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])))
                segment_completion = fraction-1+segment_completion
    return _positions


def interpolateVectorSeries(vectors, fraction):
    ''' For a series of vectors [[x1,y1],...], reinterpolate vector magnitude
        based on a fraction of magnitude
        '''
    _vectors = []
    nextIx = 0
    if(fraction < 1):
        segment_completion = 0
        while nextIx < len(vectors):
            if(1-segment_completion > fraction):
                _vectors.append((fraction*vectors[nextIx][0], fraction*vectors[nextIx][1]))
                segment_completion += fraction
            else:
                nextIx += 1
                if(nextIx >= len(vectors)): break
                _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]))
                segment_completion = fraction-1+segment_completion
    return _vectors


def 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
        '''
    pointList = [P1,P2,P3,P4]
    for point in pointList:
        if(pointList.count(point) >= 2): return point
    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]))
    x = P1[0] + ua*(P2[0] - P1[0])
    y = P1[1] + ua*(P2[1] - P1[1])
    return [x,y]


def 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
        '''
    intersections = []
    if(poly[-1] != poly[0]): poly.append(poly[0])
    for i in range(len(poly)-1):
        try: P = lineInter(P1,P2,poly[i],poly[i+1])
        except: continue
        #The intersected point is real if the intersection to point distance is shorter than the segment length for ALL points
        if(intersectEdges):
            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)
        else:
            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)
    return intersections
    
def 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
        '''
    n = len(poly)
    inside = False
    p1x,p1y = poly[0][0],poly[0][1]
    for i in range(n+1):
        p2x,p2y = poly[i % n][0],poly[i % n][1]
        if y > min(p1y,p2y):
            if y <= max(p1y,p2y):
                if x <= max(p1x,p2x):
                    if p1y != p2y:
                        xinters = (y-p1y)*(p2x-p1x)/(p2y-p1y)+p1x
                    if p1x == p2x or x <= xinters:
                        inside = not inside
        p1x,p1y = p2x,p2y
    return inside
  
def midpoint(P1,P2):
    ''' Get midpoint between two points. '''
    return [(P1[0]+P2[0])/2,(P1[1]+P2[1])/2]


def 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. '''
    # Directional vector
    vector  = [P1[cix]-O[cix] for cix in range(len(O))]
    mag     = ppd(vector)
    nvector = [vector[cix]/float(mag) for cix in range(len(O))]
    # Directional vector 2 if available
    if(PN1):
        vector2  = [O[cix]-PN1[cix] for cix in range(len(O))]
        mag     = ppd(vector2)
        nvector2 = [vector2[cix]/float(mag) for cix in range(len(O))]
        #Average nvectors
        nvector = [(nvector[cix] + nvector2[cix])/2.0 for cix in range(len(O))]
    # Scale orthogonal vector to distance (only supports 2D at this time)
    svector = [nvector[1]*distance, -nvector[0]*distance]
    # Get point
    return [O[cix]+svector[cix] for cix in range(len(O))]
    



def orthogonal_joint_midpoint(O, P1, P2):
    ''' Return a vector. '''
    #Check sides
    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):
        return midpoint(P1,P2)
    else:
        print('Error: pva_geo_tools.orthogonal_joint_midpoint() needs work...')
        return False  
        

def points_are_segmented(P1, P2, A, B):
    ''' Check if points A and B are on opposite side of a vector between P1 and P2. '''
    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
    else:                                                                                                                   return False 
    

def 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
        '''
    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]] 
    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])))
    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.")
    return m.sqrt(m.pow(x2-x1,2)+m.pow(y2-y1,2))
    
def 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.
        '''
    if((m.pow(x2-x1,2)+m.pow(y2-y1,2)) > distanceSquared): return False
    else:                                                  return True
    

def 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
        '''
    if(p0x == p1x and p0y == p1y): return None,None
    try:
        #Approximate slope singularity by giving some slope roundoff; account for roundoff error
        if(round(p0x, 10) == round(p1x, 10)):
            p1x += 0.0000000001
        if(round(p0y, 10) == round(p1y, 10)):
            p1y += 0.0000000001            
        #make the calculation
        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)))
        X = (-Y*(p1y-p0y)+qx*(p1x-p0x)+qy*(p1y-p0y))/(p1x-p0x)
    except ZeroDivisionError:
        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