Hi,
The angles you're looking for are called Euler angles.
The following routines computes the Euler angles from a transformation matrix such as the one returned by nentsel or nentselp with the two mainly used conventions: Proper Euler and Tait-Bryan.
The angles you're looking for seems to be Yaw and Pitch fromTait-Bryan angles.
;; gc:butLast
;; Returns the list but the last item
;;
;; Argument
;; l : a list
(defun gc:butLast (l) (reverse (cdr (reverse l))))
;; gc:GetNormal
;; Returns the unit vector of a vector
;; Retourne le vecteur unitaire d'un vecteur
;;
;; Argument
;; v : a vector
(defun gc:GetNormal (v)
((lambda (l)
(if (/= 0 l)
(mapcar (function (lambda (x) (/ x l))) v)
)
)
(distance '(0. 0. 0.) v)
)
)
;; TRP
;; Transposes a matrix -Doug Wilson-
;;
;; Argument
;; m : a matrix
(defun trp (m) (apply 'mapcar (cons 'list m)))
;; gc:acos
;; Returns the arc cosine of an angle
;;
;; Argument
;; n : le angle cosine
(defun gc:acos (n)
(cond
((equal n 1. 1e-9) 0.)
((equal n -1. 1e-9) pi)
((< -1. n 1.)
(atan (sqrt (- 1. (expt n 2))) n)
)
)
)
;; gc:asin
;; Returns the arc sine of an angle
;;
;; Argument
;; n : le angle sine
(defun gc:asin (n)
(cond
((equal n 1. 1e-9) (/ pi 2.))
((equal n -1. 1e-9) (/ pi -2.))
((< -1. n 1.)
(atan n (sqrt (- 1 (expt n 2))))
)
)
)
;; ProperEuler
;; Returns a list containing the the Euler angles in radians (precession, nutation and intrinsic rotation)
;;
;; Argument
;; m : a transformation matrix such as the one returned by nentsel or nentselp
(defun ProperEuler (m / theta psi phi)
(setq m (mapcar
'gc:GetNormal
(if (= (length (car m)) 3)
(mapcar 'gc:butlast (trp m))
(gc:butlast (mapcar 'gc:butlast m))
)
)
)
(setq theta (gc:acos (caddr (caddr m))))
(if (< (abs theta) 1e-7)
(setq theta 0.
psi 0.
phi (atan (car (cadr m)) (cadr (cadr m)))
)
(setq psi (atan (caddr (car m)) (- (caddr (cadr m))))
phi (atan (car (caddr m)) (cadr (caddr m)))
)
)
(list psi theta phi)
)
;; TaitBryan
;; Returns a list containing the the Tait-Bryan angles in radians (yaw, pitch, roll)
;;
;; Argument
;; m : a transformation matrix such as the one returned by nentsel or nentselp
(defun TaitBryan (m / a b c)
(setq m (mapcar
'gc:GetNormal
(if (= (length (car m)) 3)
(mapcar 'gc:butlast (trp m))
(gc:butlast (mapcar 'gc:butlast m))
)
)
)
(setq b (- (gc:asin (car (caddr m)))))
(cond
((< (abs (- b (* pi 0.5))) 1e-7)
(setq b (* pi 0.5)
a (atan (caddr (cadr m)) (cadr (cadr m)))
c 0.
)
)
((< (abs (+ b (* pi 0.5))) 1e-7)
(setq b (* pi -0.5)
a (atan (- (caddr (cadr m))) (cadr (cadr m)))
c 0.
)
)
(T
(setq a (atan (car (cadr m)) (car (car m)))
c (atan (cadr (caddr m)) (caddr (caddr m)))
)
)
)
(list a b c)
)
Here's an testing command:
(defun c:EULERANGLES (/ block matrix)
(while
(not
(and
(setq block (nentsel "\nSelect a nested entity within a block: "))
(= (length block) 4)
)
)
)
(setq matrix (caddr block))
(mapcar '(lambda (s v) (prompt (strcat "\n" s ": " (angtos v))))
'("precession" "nutation" "rotation")
(ProperEuler matrix))
(mapcar '(lambda (s v) (prompt (strcat "\n" s ": " (angtos v))))
'("yaw" "pitch" "roll")
(TaitBryan matrix))
(princ)
)
