;************************************************************************
;* [CMFAO] AME 6602 ACQUISITION DES DONNÉES SPATIALES                   *
;* [GRCAO] Claude Parisel                                               *
;* Mars 1999                                                            *
;************************************************************************
;3DR1
; calcul d'un point dans l'espace à partir de ses coordonnées dans 2 photos
; m1    matrice de pers de la 1ère photo
; m2    matrice de pers de la 2ème photo
; p1    point dans la 1ère photo [x1 y1]
; p2    point dans la 2ème photo [x2 y2]
;--------------------------------------------------------------------------
; Retour:
; point dans l'espace en format cartésien (x y z)
;--------------------------------------------------------------------------
(defun 3dr1 (m1 m2 p1 p2 / i j i1 j1 i2 j2 ma mb mc md me mx my)

  ;trouver le plan de projection de m1
  ;-----------------------------------
  (setq i1 0)
  (setq j 1)
  (while
    (<= j 3)
      (setq i 1)
      (while
        (<= i 4)
        (if
          (/= (mat-rec m1 i j) 0.0)
          (setq i 9)
        )
        (setq i (+ i 1))
      )
      (if
        (< i 9)
        (progn
          (cond
            (
              (= j 1)
              (setq i1 2)
              (setq i2 3)
            )
            (
              (= j 2)
              (setq i1 1)
              (setq i2 3)
            )
            (
              (= j 3)
              (setq i1 1)
              (setq i2 2)
            )
          )
          (setq j 3)
        )
      )
    (setq j (+ j 1))
  )
  (if (= i1 0)(print "m1 n'est pas une matrice de projection"))

  ;trouver le plan de projection de m2
  ;-----------------------------------
  (setq mx (mat-0 3 1))
  (setq j1 0)
  (setq j 1)
  (while
    (<= j 3)
      (setq i 1)
      (while
        (<= i 4)
        (if
          (/= (mat-rec m2 i j) 0.0)
          (setq i 9)
        )
        (setq i (+ i 1))
      )
      (if
        (< i 9)
        (progn
          (cond
            (
              (= j 1)
              (setq j1 2)
              (setq j2 3)
            )
            (
              (= j 2)
              (setq j1 1)
              (setq j2 3)
            )
            (
              (= j 3)
              (setq j1 1)
              (setq j2 2)
            )
          )
          (setq j 3)
        )
      )
    (setq j (+ j 1))
  )
  (if (= j1 0)(print "m2 n'est pas une matrice de projection"))

  ;calcul de ma
  ;------------------------
  (setq ma (mat-0 4 3))
  (setq i 1)
  (while
    (<=  i 3)
    (setq ma (mat-edit ma 1 i (- (mat-rec m1 i i1)(* (mat-rec m1 i 4)(car p1)))))
    (setq ma (mat-edit ma 2 i (- (mat-rec m1 i i2)(* (mat-rec m1 i 4)(cadr p1)))))
    (setq ma (mat-edit ma 3 i (- (mat-rec m2 i j1)(* (mat-rec m2 i 4)(car p2)))))
    (setq ma (mat-edit ma 4 i (- (mat-rec m2 i j2)(* (mat-rec m2 i 4)(cadr p2)))))
    (setq i (+ i 1))
  )

  ;calcul de mb
  ;------------------------
  (setq mb (mat-0 4 1))
  (setq mb (mat-edit mb 1 1 (- (* (mat-rec m1 4 4)(car p1))(mat-rec m1 4 i1))))
  (setq mb (mat-edit mb 2 1 (- (* (mat-rec m1 4 4)(cadr p1))(mat-rec m1 4 i2))))
  (setq mb (mat-edit mb 3 1 (- (* (mat-rec m2 4 4)(car p2))(mat-rec m2 4 j1))))
  (setq mb (mat-edit mb 4 1 (- (* (mat-rec m2 4 4)(cadr p2))(mat-rec m2 4 j2))))

  ;calcul de mc md me my mx
  ;------------------------
  (setq mc (mat-tpose ma))
  (setq md (mat-mul mc ma))
  (setq me (mat-inv md))
  (setq my (mat-mul mc mb))
  (setq mx (mat-mul me my))

  ;retour du point calculé format cartésien
  ;----------------------------------------
  (list (mat-rec mx 1 1)(mat-rec mx 2 1)(mat-rec mx 3 1))
)
;-----------------------------------------------------------------------