|
;************************************************************************
;* [CMFAO] AME 6602 ACQUISITION DES DONNÉES SPATIALES
*
;* [GRCAO] Claude Parisel
*
;* Mars 1999
*
;************************************************************************
; MAKE-ROT0
; construction d'une matrice de rotation autour d'un axe quelqconque
; passant par l'origine
;------------------------------------------------------------------------
; ANG Angle de rotation autour de l'axe
; PA point quelconque sur l'axe
; Xa Ya Za Coordonnées XYZ de ce point
; DENOM Dénominateur d'une division
; N1 N2 N3 3 vecteurs unitaires
; MATTROT Matrice de rotation
;------------------------------------------------------------------------
; RETOUR:
; MATTRANS Matrice finale
;---------------------------------------------------------------------
(defun make-rot0 (ang pa / xa ya za denom n1 n2 n3)
(setq ang (deg2rad ang))
;---------------------
;calcul des n: vecteurs unitaires du point pa sur les 3 axes
(setq xa (car pa) ya (cadr pa) za (caddr pa))
(setq denom (sqrt (+ (* xa xa)(* ya ya)(* za za))))
(setq n1 (/ xa denom) n2 (/ ya denom) n3 (/ za denom))
;--------------------------
;calcul de la matrice de rotation
(setq matrot
(list
(list (+ (* n1 n1)(* (- 1 (* n1 n1))(cos
ang)))
(+
(* n1 n2 (- 1 (cos ang)))(* n3 (sin ang)))
(-
(* n1 n3 (- 1 (cos ang)))(* n2 (sin ang)))
0.0
)
(list (- (* n1 n2 (- 1 (cos ang)))(*
n3 (sin ang)))
(+
(* n2 n2 )(* (- 1 (* n2 n2))(cos ang)))
(+
(* n2 n3 (- 1 (cos ang)))(* n1 (sin ang)))
0.0
)
(list (+ (* n1 n3 (- 1 (cos ang)))(*
n2 (sin ang)))
(-
(* n2 n3 (- 1 (cos ang)))(* n1 (sin ang)))
(+
(* n3 n3)(* (- 1 (* n3 n3))(cos ang)))
0.0
)
(list 0.0 0.0 0.0 1.0)
)
)
)
;---------------------------------------------------------------------------
;
Exemple:
; Command: (make-rot0 10 '( 1.0 1.0 1.0))
; ((0.989872
0.10532 -0.0951917 0.0) (-0.0951917 0.989872 0.10532 0.0)
; (0.10532
-0.0951917 0.989872 0.0) (0.0 0.0 0.0 1.0))
;----------------------------------------------------------------------------
|
