#!/usr/bin/env python
#
#

__author__ = "Randall J. Radmer"
__version__ = "1.0"
__doc__ = """Vector operations"""

import math

origin = [0.0, 0.0, 0,0]
unitX  = [1.0, 0.0, 0.0]
unitY  = [0.0, 1.0, 0.0]
unitZ  = [0.0, 0.0, 1.0]

degPerRad = 180/math.pi
radPerDeg = math.pi/180

def sum3(vA, vB):
    return (vA[0]+vB[0], vA[1]+vB[1], vA[2]+vB[2])

def delta3(vA, vB):
    return (vA[0]-vB[0], vA[1]-vB[1], vA[2]-vB[2])

def mag3(v):
    return math.sqrt(dotProd3(v, v))

def dist3(vA, vB):
    return mag3(delta3(vA, vB))

def unitVec3(v):
    return scale3(v, 1/mag3(v))

def scale3(v, k, a=[0,0,0]):
    return (k*v[0]+a[0], k*v[1]+a[1], k*v[2]+a[2])

def dotProd3(vA, vB):
    return vA[0]*vB[0]+vA[1]*vB[1]+vA[2]*vB[2]

def dotProd(vA, vB):
    t=0
    for ii in range(len(vA)):
        t+=vA[ii]*vB[ii]
    return t

def crossProd3(vA, vB):
    return (vA[1]*vB[2]-vA[2]*vB[1],
            vA[2]*vB[0]-vA[0]*vB[2],
            vA[0]*vB[1]-vA[1]*vB[0])

def normalVec3(p1, p2, p3):
    return unitVec3(crossProd3(delta3(p1, p2), delta3(p3, p2)))


def multMat3Vec3(M, v):
    return (dotProd3(M[0], v),
            dotProd3(M[1], v),
            dotProd3(M[2], v))

def multMatVec3(M, v0):
    v=[]
    for ii in range(len(M)):
        v.append(dotProd3(M[ii], v0))
    return v

def multMat3Mat3(MA, MB):
    MBt = transposeMat3(MB)
    M = []
    for ii in range(3):
        M.append([])
        for jj in range(3):
            M[-1].append(dotProd3(MA[ii], MBt[jj]))
    return M

def multMatMat(MA, MB):
    MBt = transposeMat(MB)
    M = []
    for ii in range(len(MA)):
        M.append([])
        for jj in range(len(MBt)):
            M[-1].append(dotProd(MA[ii], MBt[jj]))
    return M

def transposeMat3(M):
    return [[M[0][0], M[1][0], M[2][0]],
            [M[0][1], M[1][1], M[2][1]],
            [M[0][2], M[1][2], M[2][2]]]

def transposeMat(M0):
    M=[]
    for jj in range(len(M0[0])):
        v=[]
        for ii in range(len(M0)):
            v.append(M0[ii][jj])
        M.append(v)
    return M

def trans3(p, v, sign=1):
    return sum3(p, scale3(v, sign))

def rotMat3(pA, pB, angle):
    # build rotation matrix using quaternions
    # see 'Computer Graphics', by D. Hearn and M.P. Baker, Second Edition,
    # page 419.

    s = math.cos(angle/2.0)
    v = scale3(unitVec3(delta3(pA, pB)), math.sin(angle/2))

    aa = 2*v[0]*v[0]; bb = 2*v[1]*v[1]; cc = 2*v[2]*v[2]
    ab = 2*v[0]*v[1]; ac = 2*v[0]*v[2]; bc = 2*v[1]*v[2]
    sa = 2*v[0]*s;    sb = 2*v[1]*s;    sc = 2*v[2]*s

    M = [[1-bb-cc,   ab-sc,   ac+sb],
         [  ab+sc, 1-aa-cc,   bc-sa],
         [  ac-sb,   bc+sa, 1-aa-bb]]
    return M

def rotVec3(pA, pB, angle, vec):
    return multMat3Vec3(rotMat3(pA, pB, angle), vec)




if __name__=='__main__':
    import sys
 
    vXY = sum3(unitX, unitY)
    sys.stdout.write('vXY = %s, mag3(vXY) = %0.4f, 2*vXY = %0s\n'
        % (str(vXY), mag3(vXY), scale3(vXY, 2)))

    vXZ = sum3(unitX, unitZ)
    sys.stdout.write('vXZ = %s, mag3(vXZ) = %0.4f, -0.5*vXZ = %s\n'
        % (str(vXZ), mag3(vXZ), scale3(vXZ, -0.5)))

    vXYZ = sum3(vXY, unitZ)
    sys.stdout.write('vXYZ = %s, mag3(vXYZ) = %0.4f, vXYZ+[-1,0,1] = %s\n'
        % (str(vXYZ), mag3(vXYZ), scale3(vXYZ, 1, [-1, 0, 1])))

    sys.stdout.write('vXY+vZ = %s, vXY-vZ %s\n'
        % (str(sum3(vXY, unitZ)), str(delta3(vXY, unitZ))))
    sys.stdout.write('vXY<dot>vZ = %0.4f, vXY<cross>vZ = %s\n'
        % (dotProd3(vXY, unitZ), str(crossProd3(vXY, unitZ))))

    sys.stdout.write('vX<cross>vY = %s, vX<cross>vZ = %s\n'
        % (str(crossProd3(unitX, unitY)), str(crossProd3(unitX, unitZ))))

    M = rotMat3(unitY, vXY, math.pi/2)
    sys.stdout.write('rotMat3(xAxis, pi/2):\n  %s\n  %s\n  %s\n'
        % (str(M[0]), str(M[1]), str(M[2])))

    M = rotMat3(origin, vXYZ, 2*math.pi/3)
    sys.stdout.write('rotMat3(vXYZ, 2*pi/3):\n  %s\n  %s\n  %s\n'
        % (str(M[0]), str(M[1]), str(M[2])))