XD::Mat:DispToMatrix
;;; XD::Mat:DispToMatrix
;;; 描述：
;;; 将一个旋转矩阵 mat 和一个位移向量 disp 组合成一个 4x4 仿射变换矩阵。
;;; 支持输入为 3x3 或 4x4 形式的旋转矩阵。
;;;
;;; 调用格式：
;;; (XD::Mat:DispToMatrix mat disp)
;;;
;;; 参数说明：
;;; mat- 旋转矩阵（3x3 或 4x4 列表）
;;; disp - 位移向量（3 个数值组成的列表）
;;;
;;; 返回值：
;;; 返回一个 4x4 仿射变换矩阵（嵌套列表）
(defun XD::Mat:DispToMatrix (mat disp)
(cond
;; 如果是 3x3 矩阵，扩展为 4x4 仿射矩阵
((and (= (length (car mat)) 3) (= (length mat) 3))
(append
(mapcar 'append mat (mapcar 'list disp))
'((0. 0. 0. 1.))
)
)
;; 如果已经是 4x4 矩阵，仅更新位移部分
((and (= (length (car mat)) 4) (= (length mat) 4))
(mapcar
'(lambda (x y) (list (car x) (cadr x) (caddr x) (+ (cadddr x) y)))
mat
(append disp (list 0.0)); 保证第4行不变
)
)
)
)
学习一下代码，有用。感谢分享。
页:
[1]