import numpy as np
#import cv
import cv2


def MatRot(Rvec):   # Convierte un vector de rotacion en una matriz de rotacion                 
     ang=np.linalg.norm(Rvec)
     Rvec=Rvec/ang
     I=np.identity(3)
     rx=Rvec[0,0]
     ry=Rvec[1,0]
     rz=Rvec[2,0]
     R=np.cos(ang)*I+(1-np.cos(ang))*np.dot(Rvec,Rvec.T)+np.sin(ang)*np.array([[0,-rz,ry],[rz,0,-rx],[-ry,rx,0]])
     return R

def AngRot(MatR,grados=0):		# Calcula la rotacion alrededor de cada eje conociendo la matriz de rotacion                
	ret,mtR,mtQ,MRx,MRy,MRz=cv2.RQDecomp3x3(MatR)	

	Rx = np.angle(np.complex(MRx[1,1],MRx[2,1]))
	Ry = np.angle(np.complex(MRy[0,0],MRy[0,2]))
	Rz = np.angle(np.complex(MRz[0,0],MRz[1,0]))		
	if grados==1:
		Rx=Rx*180/np.pi
		Ry=Ry*180/np.pi
		Rz=Rz*180/np.pi
	Rvec=[Rx,Ry,Rz]	
	return Rvec


def get_homog(Tvec,Rvec):
	Rvec = Rvec*np.pi/180
	H = np.identity(4)
	Hx = np.array([[1,0,0],
		[0,np.cos(Rvec[0]),-np.sin(Rvec[0])],
		[0,np.sin(Rvec[0]),np.cos(Rvec[0])]])
	Hy = np.array([[np.cos(Rvec[1]),0,np.sin(Rvec[1])],
		[0,1,0],
		[-np.sin(Rvec[1]),0,np.cos(Rvec[1])]])
	Hz = np.array([[np.cos(Rvec[2]),-np.sin(Rvec[2]),0],
		[np.sin(Rvec[2]),np.cos(Rvec[2]),0],
		[0,0,1]])
	HR = np.dot(Hx,np.dot(Hy,Hz))
	H[:3,3] = Tvec 
	H[:3,:3]= HR
	return H
	

