import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits import mplot3d

from matplotlib.patches import FancyArrowPatch
from mpl_toolkits.mplot3d import proj3d

#Matrice Rot Asse Z
def RotZ(theta):
    theta = np.radians(theta)
    c, s = np.cos(theta), np.sin(theta)
    R=np.array(((c,-s,0),(s,c,0),(0,0,1)))
    return R
#Matrice Rot Asse X
def RotX(theta):
    theta = np.radians(theta)
    c, s = np.cos(theta), np.sin(theta)
    R=np.array(((1,0,0),(0,c,-s),(0,s,c)))
    return R
#Matrice Rot Asse Y
def RotY(theta):
    theta = np.radians(theta)
    c, s = np.cos(theta), np.sin(theta)
    R=np.array(((c,0,s),(0,1,0),(-s,0,c)))
    return R
#Disegno di Freccia in 3D
class Arrow3D(FancyArrowPatch):
    def __init__(self, xs, ys, zs, *args, **kwargs):
        FancyArrowPatch.__init__(self, (0,0), (0,0), *args, **kwargs)
        self._verts3d = xs, ys, zs

    def draw(self, renderer):
        xs3d, ys3d, zs3d = self._verts3d
        xs, ys, zs = proj3d.proj_transform(xs3d, ys3d, zs3d, renderer.M)
        self.set_positions((xs[0],ys[0]),(xs[1],ys[1]))
        FancyArrowPatch.draw(self, renderer)
#disegna un frame XYZ con origine in O e mayrice pos in H
def DrawFrame(axm,O=[0,0,0],H=[[1,0,0],[0,1,0],[0,0,1]],OLabel='',XLabel='',YLabel='',ZLabel=''):
    if type(O) is np.ndarray:
        O=O.tolist()[0]
    if type(H) is np.ndarray:
        H=H.tolist()
    LX=ax.get_xlim3d()[1]
    a = Arrow3D([O[0],O[0]+H[0][0]*LX],[O[1],O[1]+H[1][0]*LX],[O[2],O[2]+H[2][0]*LX], mutation_scale=20,  arrowstyle="-|>", color="r")
    ax.add_artist(a)
    LY=ax.get_ylim3d()[1]
    a =  Arrow3D([O[0],O[0]+H[0][1]*LY],[O[1],O[1]+H[1][1]*LY],[O[2],O[2]+H[2][1]*LY], mutation_scale=20,  arrowstyle="-|>", color="g")
    ax.add_artist(a)
    LZ=ax.get_zlim3d()[1]
    a =  Arrow3D([O[0],O[0]+H[0][2]*LZ],[O[1],O[1]+H[1][2]*LZ],[O[2],O[2]+H[2][2]*LZ],  mutation_scale=20,  arrowstyle="-|>", color="b")
    ax.add_artist(a)

    ax.text(O[0],O[1],O[2],OLabel)
    ax.text(O[0]+H[0][0]*LX,O[1]+H[1][0]*LX,O[2]+H[2][0]*LX, XLabel)
    ax.text(O[0]+H[0][1]*LY,O[1]+H[1][1]*LY,O[2]+H[2][1]*LY,YLabel)
    ax.text(O[0]+H[0][2]*LZ,O[1]+H[1][2]*LZ,O[2]+H[2][2]*LZ,ZLabel)
#disegna un frame XYZ Passando come parametro una matrice omogenea 
def DrawFrameH(ax,H=[[1,0,0,0],[0,1,0,0],[0,0,1,0],[0, 0, 0, 1]],OLabel='',XLabel='',YLabel='',ZLabel=''):
    if(type(H)==list):
        H=np.array(H)
    O=H[0:3:1,[3]].T
    H=H[0:3:1, 0:3:1]
    DrawFrame(ax,O=O,H=H,OLabel=OLabel,XLabel=XLabel,YLabel=YLabel,ZLabel=ZLabel)

#Completa una matrice 3x3 in omogenea 4x4
def toH(M):
    if type(M) == list:
        M=np.array(M)
    d=len(np.shape(M))
    if d==1 and len(M)==3:
        return np.append(M, [1]) 
    c = np.array([[0], [0], [0]])
    MH = np.append(M, c, axis=1)
    r = np.array([[0,0,0,1]])
    MH = np.append(MH, r, axis=0)
    return MH

#inserisce un vettore di traslazione in una matrice omogenea
def HInsT(H,T):
    for i in range(4-1):
        H[i,3]=T[i]
    return H

#restituisce una matrice quadrata identica (dxd)       
def identity(d):
    z = np.zeros( (d, d) )
    for i in range(d):
        z[i,i]=1
    return z    


############################################# Esercizi ###################################
def esPag46(ax):
    P=[[0,0,0],[2,0,0],[2,1,0],[0,1,0],[0,1,2],[0,0,2],[1,0,2],[1,1,2],[0,1,2],[1,1,2],[1,1,1],[1,0,1],[1,0,2],
        [1,0,1],[2,0,1],[2,0,0],[2,1,0],[2,1,1],[2,0,1],[2,1,1],[1,1,1],[1,0,1],[1,0,2],[0,0,2],[0,0,0],[0,1,0]]
    p1=P[0]

    for p in P:       
        ax.plot([p1[0],p[0]],[p1[1],p[1]],zs=[p1[2],p[2]])
        p1=p
 
    R=RotZ(90)
    T=np.array([1,5,3])
    Q = [R.dot(p)+T for p in P]
    p1=Q[0]
    for p in Q:       
        ax.plot([p1[0],p[0]],[p1[1],p[1]],zs=[p1[2],p[2]])
        p1=p
        
def esPag52(ax):
    d=10
    ax.set_xlim3d(-d,d)
    ax.set_ylim3d(-d,d)
    ax.set_zlim3d(-d,d)
    p=[1,6,3]
    X=p[0]
    Y=p[1]
    Z=p[2]
    print X,Y,Z
    ax.plot([X], [Y], [Z],markersize=8,markerfacecolor='k',marker='.', markeredgecolor='k')
    
    R=RotZ(90)
    q=R.dot(p)
    X=q[0]
    Y=q[1]
    Z=q[2]
    ax.plot([X], [Y], [Z],markersize=8,markerfacecolor='r',marker='.', markeredgecolor='k')
    
def esPag64(ax):
    DrawFrame(ax)
    H = np.array([[1, 0, 0, 2],
                  [0, 1, 0, 4],
                  [0, 0, 1, 4],
                  [0, 0, 0, 1]])
    p=[5,1,2,1]
    X=p[0]
    Y=p[1]
    Z=p[2]
    ax.plot([X], [Y], [Z],markersize=8,markerfacecolor='k',marker='.', markeredgecolor='k')
    q = H.dot(p)
    X=q[0]
    Y=q[1]
    Z=q[2]
    ax.plot([X], [Y], [Z],markersize=8,markerfacecolor='r',marker='.', markeredgecolor='k')


def esPag66(ax):
    d=10
    ax.set_xlim3d(-d,d)
    ax.set_ylim3d(-d,d)
    ax.set_zlim3d(-d,d)
    R = RotZ(90)
    RH=toH(R)
    p = [1,6,3,1]
    DrawFrame(ax)
    X=p[0]
    Y=p[1]
    Z=p[2]
    ax.plot([X], [Y], [Z],markersize=8,markerfacecolor='k',marker='.', markeredgecolor='k')

    q=RH.dot(p)
    X=q[0]
    Y=q[1]
    Z=q[2]
    ax.plot([X], [Y], [Z],markersize=8,markerfacecolor='r',marker='.', markeredgecolor='k')

def esPag68(ax):
    d=10
    ax.set_xlim3d(-d,d)
    ax.set_ylim3d(-d,d)
    ax.set_zlim3d(-d,d)
    RX=toH(RotX(90))
    RY=toH(RotY(90))
    RZ=toH(RotZ(90))
    p=toH([1,3,5])
    p1=RX.dot(p)
    p2=RY.dot(p1)
    T=toH([5,10,2])
    H=identity(4)
    T = HInsT(H,[5,10,2])
    p3=T.dot(p2)
    X=p[0]
    Y=p[1]
    Z=p[2]
    ax.plot([X], [Y], [Z],markersize=8,markerfacecolor='k',marker='.', markeredgecolor='k')    
    X=p1[0]
    Y=p1[1]
    Z=p1[2]
    ax.plot([X], [Y], [Z],markersize=8,markerfacecolor='y',marker='.', markeredgecolor='k')
    X=p2[0]
    Y=p2[1]
    Z=p2[2]
    ax.plot([X], [Y], [Z],markersize=8,markerfacecolor='r',marker='.', markeredgecolor='k')
    X=p3[0]
    Y=p3[1]
    Z=p3[2]
    ax.plot([X], [Y], [Z],markersize=8,markerfacecolor='b',marker='.', markeredgecolor='k')
    DrawFrame(ax)
    
def esPag71(ax):
    d=40
    ax.set_xlim3d(-d,d)
    ax.set_ylim3d(-d,d)
    ax.set_zlim3d(-d,d)
    M=np.array([[0, 1, 0, 5],
                [0, 0, -1, 10],
                [-1, 0, 0, 2],
                [0, 0, 0, 1]])
    I=identity(4)
    H=M.dot(I)
    DrawFrameH(ax,H=H)

def esPag72(ax):
    d=40
    ax.set_xlim3d(-d,d)
    ax.set_ylim3d(-d,d)
    ax.set_zlim3d(-d,d)
    O=np.array([[0, -1, 0, 2],
                [1,  0, 0, 4],
                [0,  0, 1, 4],
                [0,  0, 0, 1]])
    RH = toH(RotX(90))
    H=RH.dot(O)
    DrawFrameH(ax,H=H,OLabel='O')
    RH = toH(RotX(90))
    H=O.dot(RH)
    DrawFrameH(ax,H=H,OLabel="O'")
    print H


    
np.set_printoptions(precision=2,suppress=True)

fig = plt.figure()
ax = fig.gca(projection='3d')

ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
ax.set_xlim3d(0,10)
ax.set_ylim3d(0,10)
ax.set_zlim3d(0,10)

esPag72(ax)

plt.show()