def Vector (x,y,z):
    return (x,y,z)
def Add (a,b):
    return (a[0]+b[0],a[1]+b[1],a[2]+b[2])
def Sub (a,b):
    return (a[0]-b[0],a[1]-b[1],a[2]-b[2])
def Neg (a):
    return (-a[0],-a[1],-a[2])
def Cross(a,b):
    return (a[1]*b[2]-a[2]*b[1], a[2]*b[0]-a[0]*b[2], a[0]*b[1]-a[1]*b[0])
def Scale(a,f):
    return (a[0]*f,a[1]*f,a[2]*f)
def Dot (a,b):
    return a[0]*b[0]+a[1]*b[1]+a[2]*b[2]
def Transform (a,p,q,r):
    return (Dot (a,p),
            Dot (a,q),
            Dot (a,r))
def Mag(a):
    import VS
    return VS.sqrt(Dot(a,a))
def Norm (a):
    return Scale(a,1.0/Mag(a))
def SafeNorm (a):
    mag = Mag(a)
    if (mag):
        return Scale(a,1.0/mag)
    else:
        return Vector(0,0,0)
def ScaledCross (a,b):
    return Norm(Cross(a,b))
def Max(a,b):
    return ((a[0]>b[0])*a[0]+(a[0]<=b[0])*b[0],
            (a[1]>b[1])*a[1]+(a[1]<=b[1])*b[1],
            (a[2]>b[2])*a[2]+(a[2]<=b[2])*b[2])
def Min(a,b):
    return ((a[0]<b[0])*a[0]+(a[0]>=b[0])*b[0],
            (a[1]<b[1])*a[1]+(a[1]>=b[1])*b[1],
            (a[2]<b[2])*a[2]+(a[2]>=b[2])*b[2])


############################################## PODXXX
import math
import vsrandom


def calcDistance( p1, p2):
        dx = p1[0] - p2[0]
        dy = p1[1] - p2[1]
        dz = p1[2] - p2[2]
        return math.sqrt(dx**2 + dy**2 + dz**2)
       
       
def findMiddle( p1, p2):
        return ( .5*p1[0] + .5*p2[0], .5*p1[1] + .5*p2[1], .5*p1[2] + .5*p2[2])

        
def coordBetweenByRatio( c1, c2, r):
        if c1 >= c2:
            return c1 - (c1 - c2) * r
        else:
            return c1 + (c2 - c1) * r
        
        
def posBetweenByRatio( p1, p2, r):
        return ( coordBetweenByRatio( p1[0], p2[0], r),
                 coordBetweenByRatio( p1[1], p2[1], r),
                 coordBetweenByRatio( p1[2], p2[2], r))


def randomizeSpotBetween( np1pos, np2pos):
        mp = findMiddle( np1pos, np2pos)
        halfDistance = calcDistance( np2pos, mp)
        margin = 3500     #min distance from each navpoint
        spawnRange = vsrandom.random() * ( halfDistance - margin)    #distance of the spawn spot from the middle point
        if vsrandom.random() >= .5:
            spawnRatio = (halfDistance + spawnRange) / calcDistance( np1pos, np2pos)
        else:
            spawnRatio = (halfDistance - spawnRange) / calcDistance( np1pos, np2pos)
        spawnCenter = posBetweenByRatio( np1pos, np2pos, spawnRatio)     #spawn center coordinates
        spawnRadius = 1000        #delimits the spawn area around the spawn center, will reduce it if overlaps with margins
        if (spawnRange + spawnRadius + margin > halfDistance):
            spawnRadius -= spawnRange + spawnRadius + margin - halfDistance
        fi = vsrandom.random() * math.radians(360)
        z = vsrandom.random()*2*spawnRadius - spawnRadius
        r = math.sqrt( spawnRadius*spawnRadius - z*z)
        x = r * math.cos( fi)
        y = r * math.sin( fi)
        return ( x + spawnCenter[0], y + spawnCenter[1], z + spawnCenter[2])

        
def randomizeSpotAround( pos, minR, maxR):
        if maxR - minR < 2:
            maxR = minR + 2
        spawnRadius = vsrandom.randrange( int( minR), int( maxR))
        z = vsrandom.randrange( -spawnRadius, spawnRadius)
        r = math.sqrt( spawnRadius*spawnRadius - z*z)
        fi = vsrandom.random() * math.radians(360)
        x = r * math.cos( fi)
        y = r * math.sin( fi)
        return ( x + pos[0], y + pos[1], z + pos[2])

