# jch 31-OCT-2005; created TinyVector.py from Point.py utility
# $Header: /jch/play/cvsroot/twist/Point.py,v 1.2 2003/05/30 03:20:04 jch Exp $
#// Copyright 2003 YON - Jan C. Hardenbergh. Permission to copy is
#// granted as long as this copyright and this URL are maintained:
#// http://www.jch.com/NURBS/
# http://www.j3d.org/matrix_faq/matrfaq_latest.html
# This is statement is required by the build system to query build info
if __name__ == '__build__':
	raise Exception

import Numeric
import math
from TinyVector import TinyVector

def GetAngleFromTwoMatricies(array1, array2):
    # hack #1 assume rotate around Y axis - is that true?
    # and if we make that asumption, can we just do atan2(array[2], array[0])
    refVector = TinyVector([1,0,0])

    matrix1 = Numeric.array([ [array1[0], array1[1], array1[2]], [array1[3], array1[4], array1[5]], [array1[6], array1[7], array1[8]] ])
    res = Numeric.matrixmultiply(matrix1, refVector.GetVertex())
    vec1 = TinyVector(res)
    vec1.Print()

    matrix2 = Numeric.array([ [array2[0], array2[1], array2[2]], [array2[3], array2[4], array2[5]], [array2[6], array2[7], array2[8]] ])
    res2 = Numeric.matrixmultiply(matrix2, refVector.GetVertex())
    vec2 = TinyVector(res2)
    vec2.Print()

    cos = vec1.Dot(vec2)
    xvec = vec1.Cross(vec2)
    xvec.Print()
    sin = xvec.Length()
    if xvec.Dot([0,1,0]) < 0: # hack #2 is this right? else flip it.
	sin = -sin
    print cos, sin
    angle = math.atan2(sin,cos)
    return (angle/3.1415927)*180


def main():

    time0 = Numeric.array([-1,0,0, 0,1,0, 0,0,-1])
    time4800 = Numeric.array([-0.866,0,0.5, 0,1,0, -0.5,0,-0.866])
    fortyFive = Numeric.array([-0.707,0,0.707, 0,1,0, -0.707,0,-0.707])

    angle = GetAngleFromTwoMatricies(time0,time4800)
    print "angle is %g"%(angle)

    angle = GetAngleFromTwoMatricies(time4800,time0)
    print "angle 2 is %g"%(angle)

    angle = GetAngleFromTwoMatricies(time0,fortyFive)
    print "angle 3 is %g"%(angle)

    angle = GetAngleFromTwoMatricies(fortyFive,time4800)
    print "angle 4 is %g"%(angle)


if __name__ == '__main__':
    main()