#!/usr/bin/python3
import cv2
import numpy as np
import math
print("==============================================================")
#modèle 3D filaire d'un cube:
taillecube=1 #unité métrique pour le coté du cube
listeM = np.float32([[0,0,0], [taillecube,0,0],[taillecube,taillecube,0], [0,taillecube,0],
                    [0,0,taillecube], [taillecube,0,taillecube],[taillecube,taillecube,taillecube], [0,taillecube,taillecube]])
print("listeM:"+str(listeM))
listeseg=[[0,1],[1,2],[2,3],[3,0],[4,5],[5,6],[6,7],[7,4],[0,4],[1,5],[2,6],[3,7]]                   
print("listeseg:"+str(listeseg))
print("==============================================================")



    #https://docs.opencv.org/2.4/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html?highlight=pnp#void%20Rodrigues(InputArray%20src,%20OutputArray%20dst,%20OutputArray%20jacobian)
rvecs=(math.pi/4.)*np.float32([0,0,1])
print(str(rvecs))
rot, jacobian=cv2.Rodrigues(rvecs)
print(str(rot))
print(str(jacobian))
tvecs=np.float32([[3,-1,5]]).transpose()
print(str(tvecs))

cRTw=np.concatenate(  (rot,tvecs)  ,axis=1)
print(str(cRTw))
cRTw=np.concatenate(  (cRTw,np.float32([[0,0,0,1]])) ,axis=0)
print(str(cRTw))
#=========================================================
eu=0.00001
ev=eu
largeur=800
hauteur=600
fovhdegres=120
demifovhradian=(fovhdegres/2.)*(math.pi/180.)
pu=largeur/2
pv=hauteur/2
alphau=pu/math.tan(demifovhradian)
print(str(alphau))
f=alphau*eu
print(str(f))

alphav=f/ev


iCc=np.float32([[alphau,0,pu,0],[ 0, alphav,pv,0], [0,0,1,0]])
print(str(iCc))

iCw=iCc@cRTw
print(str(iCw))


#############################
def projeterPoint(M):
    print("M:"+str(M))
    M=np.append(M,1)
    print("M:"+str(M))   
    m=iCw@M
    print("m:"+str(m))   
    #m=m*(1/m[2])
    mu=m[0]/m[2]
    mv=m[1]/m[2]
    #arrondi et cast
    mu=int(round(mu,0))
    mv=int(round(mv,0))
    print("mu:"+str(mu))   
    print("mv:"+str(mv))   
    return (mu,mv)

#############################

im=np.zeros((hauteur,largeur),np.uint8)

for i in range(len(listeM)):
    (mu,mv)=projeterPoint(listeM[i])
    im[mv,mu]=255



for i in range(len(listeseg)):
    (mu1,mv1)=projeterPoint(listeM[listeseg[i][0]])
    (mu2,mv2)=projeterPoint(listeM[listeseg[i][1]])

    cv2.line(im,(mu1,mv1),(mu2,mv2),255,1,cv2.LINE_AA)

cv2.imwrite('imgcube2025.png',im)

cv2.imshow('image',im)
cv2.waitKey(0)
cv2.destroyAllWindows()