# test_rotate.py3  use matrix multiply
from math import *
from numpy import array
from numpy import linalg
print(dir(linalg))
# The rotation matrix by angle ax about the X axis is
# | 1.0    0.0      0.0     |   | x |
# | 0.0    cos ax  -sin ax  | * | y | = | xr, yr, zr | 
# | 0.0    sin ax  cos ax   |   | z |
#
# The rotation matrix by angle ay about the Y axis is
# | cos ay   0.0    sin ay  |   | x |
# | 0.0      1.0    0.0     | * | y | = | xr, yr, zr | 
# | -sin ay  0.0    cos ay  |   | z |
#
# The rotation matrix by angle az about the Z axis is
# | cos az  -sin az  0.0    |   | x |
# | sin az  cos az   0.0    | * | y | = | xr, yr, zr | 
# | 0.0     0.0      1.0    |   | z |
#
# product of these three times old vector is new vector

def vecmul(a, p): # return pp
  pp = [0.0, 0.0, 0.0]
  for i in range(3):
    for j in range(3):
      pp[i]= pp[i] + a[i][j] * p[j];

    # end j
      
  # end i
  return pp
# end vecmul

print("test_rotate.py3 running")
ax = pi/4.0
print("ax=",ax)
ay = pi/4.0
print("ay=",ay)
az = pi/4.0
print("az=",az)
print("sin(ax)=",sin(ax))
print("cos(ax)=",cos(ax))
# The rotation matrix by angle ax about the X axis is
xm = array([[1.0, 0.0, 0.0],[0.0, cos(ax), -sin(ax)],[0.0, sin(ax), cos(ax)]])
print("xm=",xm)
p1 = [1.0, 3.0, 4.0]
print("p1=",p1)
r = sqrt(p1[1]*p1[1]+p1[2]*p1[2])
print("r=",r)
p2 = vecmul(xm,p1)
print("p2=",p2)
# r = sqrt(p2[1]*p2[1]+p2[2]*p2[2])
# print("r=",r)
p3 = vecmul(xm,p2)
print("p3=",p3)
p4 = vecmul(xm,p3)
print("p4=",p4)
p5 = vecmul(xm,p4)
print("p5=",p5)
p6 = vecmul(xm,p5)
print("p6=",p6)
p7 = vecmul(xm,p6)
print("p7=",p7)
p8 = vecmul(xm,p7)
print("p8=",p8)
p9 = vecmul(xm,p8)
print("p9=",p9)

#
# The rotation matrix by angle ay about the Y axis is
# | cos ay   0.0    sin ay  |   | x |
# | 0.0      1.0    0.0     | * | y | = | xr, yr, zr | 
# | -sin ay  0.0    cos ay  |   | z |
ym = array([[cos(ay), 0.0, sin(ay)],[0.0, 1.0, 0.0],[-sin(ay), 0.0, cos(ay)]])
print("ym=",ym)
p1 = [3.0, 1.0, 4.0]
print("p1=",p1)
r = sqrt(p1[1]*p1[1]+p1[2]*p1[2])
print("r=",r)
p2 = vecmul(ym,p1)
print("p2=",p2)
# r = sqrt(p2[1]*p2[1]+p2[2]*p2[2])
# print("r=",r)
p3 = vecmul(ym,p2)
print("p3=",p3)
p4 = vecmul(ym,p3)
print("p4=",p4)
p5 = vecmul(ym,p4)
print("p5=",p5)
p6 = vecmul(ym,p5)
print("p6=",p6)
p7 = vecmul(ym,p6)
print("p7=",p7)
p8 = vecmul(ym,p7)
print("p8=",p8)
p9 = vecmul(ym,p8)
print("p9=",p9)
#
# The rotation matrix by angle az about the Z axis is
# | cos az  -sin az  0.0    |   | x |
# | sin az  cos az   0.0    | * | y | = | xr, yr, zr | 
# | 0.0     0.0      1.0    |   | z |
zm = array([[cos(az), -sin(az), 0.0],[sin(az), cos(az), 0.0],[0.0, 0.0, 1.0]])
print("zm=",zm)
p1 = [3.0, 4.0, 1.0]
print("p1=",p1)
r = sqrt(p1[0]*p1[0]+p1[1]*p1[1])
print("r=",r)
p2 = vecmul(zm,p1)
print("p2=",p2)
# r = sqrt(p2[1]*p2[1]+p2[2]*p2[2])
# print("r=",r)
p3 = vecmul(zm,p2)
print("p3=",p3)
p4 = vecmul(zm,p3)
print("p4=",p4)
p5 = vecmul(zm,p4)
print("p5=",p5)
p6 = vecmul(zm,p5)
print("p6=",p6)
p7 = vecmul(zm,p6)
print("p7=",p7)
p8 = vecmul(zm,p7)
print("p8=",p8)
p9 = vecmul(zm,p8)
print("p9=",p9)
print("test_rotate.py3 finished")