;************************************************************************
;* [CMFAO] AME 6602 ACQUISITION DES DONNÉES SPATIALES                   *
;* [GRCAO] Claude Parisel                                               *
;* Mars 1999                                                            *
;************************************************************************
;3DR2- 3D reconstruction 2
;Reconstruction d'une matrice de transformation 3D to perspective à partir
;de 6 points homologues dans l,espace et dans la perspective.
;-------------------------------------------------------------------------
;mat_o  matrice de 6x3 de point d'objets
;mat_p  matrice de 6x2 de points perpectifs
;ma     matrice temporaire 12x12
;mb     matrice temporaire 11x11
;mc     matrice temporaire 11x1
;me     matrice temporaire
;md     matrice temporaire
;-------------------------------------------------------------------------
;Retour:
;mtmatrice de transformation (4x4)
;-------------------------------------------------------------------------
(defun 3dr2 (mat_o mat_p / ma mb mc md me i j)

  ;initialisation des matrices mt et ma
  ;------------------------------------
  (setq mt (mat-0 4 4));t(,)
  (setq ma (mat-0 12 12));A(,)
  ;construction de la matrice A (ma)
  ;------------------------------------
  (setq i 1 j 1)
  (while
    (<= i 12)
    (setq ma (mat-edit ma i 1 (mat-rec mat_o j 1)))
    (setq ma (mat-edit ma (+ i 1) 2 (mat-rec mat_o j 1)))
    (setq ma (mat-edit ma i 4 (mat-rec mat_o j 2)))
    (setq ma (mat-edit ma (+ i 1) 5 (mat-rec mat_o j 2)))
    (setq ma (mat-edit ma i 7 (mat-rec mat_o j 3)))
    (setq ma (mat-edit ma (+ i 1) 8 (mat-rec mat_o j 3)))
    (setq ma (mat-edit ma i 10 1))
    (setq ma (mat-edit ma (+ i 1) 11 1))
    (setq ma (mat-edit ma i 12 (mat-rec mat_p j 1)))
    (setq ma (mat-edit ma (+ i 1) 12 (mat-rec mat_p j 2)))
    (setq ma (mat-edit ma i 3 (* (- (mat-rec mat_o j 1))(mat-rec mat_p j 1))))
    (setq ma (mat-edit ma (+ i 1) 3 (* (- (mat-rec mat_o j 1))(mat-rec mat_p j 2))))
    (setq ma (mat-edit ma i 6 (* (- (mat-rec mat_o j 2))(mat-rec mat_p j 1))))
    (setq ma (mat-edit ma (+ i 1) 6 (* (- (mat-rec mat_o j 2))(mat-rec mat_p j 2))))
    (setq ma (mat-edit ma i 9 (* (- (mat-rec mat_o j 3))(mat-rec mat_p j 1))))
    (setq ma (mat-edit ma (+ i 1) 9 (* (- (mat-rec mat_o j 3))(mat-rec mat_p j 2))))
    (setq i (+ i 2))
    (setq j (+ j 1))
  )

  ;initialisation des matrices mb et mc
  ;------------------------------------
  (setq mb (mat-0 11 11) mc (mat-0 11 1))

  ;calcul des matrice mb et mc
  ;---------------------------
  (setq i 1)
  (while
    (<= i 11)
    (setq j 1)
    (while
      (<= j 11)
      (setq mb (mat-edit mb i j (mat-rec ma i j)))
      (setq j (+ j 1))
    )
    (setq mc (mat-edit mc i 1 (mat-rec ma i 12)))
    (setq i (+ i 1))
  )

  ;calcul de la matrice me inverse de mb et md
  ;-------------------------------------------
  (setq me (mat-inv mb))
  (setq md (mat-mul me mc))

  ;calcul de la matrice de transformation 4x4 mt
  ;---------------------------------------------
  (setq mt (mat-0 4 4))
  (setq mt (mat-edit mt 1 1 (mat-rec md 1 1)))
  (setq mt (mat-edit mt 1 2 (mat-rec md 2 1)))
  (setq mt (mat-edit mt 1 4 (mat-rec md 3 1)))
  (setq mt (mat-edit mt 2 1 (mat-rec md 4 1)))
  (setq mt (mat-edit mt 2 2 (mat-rec md 5 1)))
  (setq mt (mat-edit mt 2 4 (mat-rec md 6 1)))
  (setq mt (mat-edit mt 3 1 (mat-rec md 7 1)))
  (setq mt (mat-edit mt 3 2 (mat-rec md 8 1)))
  (setq mt (mat-edit mt 3 4 (mat-rec md 9 1)))
  (setq mt (mat-edit mt 4 1 (mat-rec md 10 1)))
  (setq mt (mat-edit mt 4 2 (mat-rec md 11 1)))
  (setq mt (mat-edit mt 4 4 1))

  ;retour
  ;------
  mt
)
;--------------------------------------------------------------------------