XD::Mat:AlignToWCS
;;; XD::Mat:AlignToWCS
;;; 描述：
;;; 根据给定的原点 Org 和两个方向向量 Vx、Vy，生成将自定义坐标系（UCS）与世界坐标系（WCS）对齐的变换矩阵。
;;; 返回从 UCS → WCS 以及 WCS → UCS 的两个 4x4 仿射矩阵。
;;;
;;; 调用格式：
;;; (XD::Mat:AlignToWCS Org Vx Vy)
;;;
;;; 参数说明：
;;; Org - 原点（3D 点列表），如 (0 0 0)
;;; Vx- X 轴方向向量（3D 向量列表）
;;; Vy- Y 轴方向向量（3D 向量列表）
;;;
;;; 返回值：
;;; 若 Vx 或 Vy 为零向量，返回单位矩阵；
;;; 否则返回一个包含两个矩阵的列表：(list m1 m2)
;;; m1：UCS → WCS 变换矩阵
;;; m2：WCS → UCS 变换矩阵
(defun XD::Mat:AlignToWCS (Org Vx Vy / Vz Rot ang Rot1 Tr m1 m2)
(if (or (equal Vx '(0 0 0) 1e-14) (equal Vy '(0 0 0) 1e-14))
'((1. 0. 0. 0.) (0. 1. 0. 0.) (0. 0. 1. 0.) (0. 0. 0. 1.))
(progn
(setq Vx(XD::Vec:unit Vx)) ; X Axis
(setq Vy(XD::Vec:unit Vy)) ; Y Axis
(setq Vz(XD::Vec:unit (XD::Vec:vxv Vx Vy))) ; Z Axis
(setq Vy(XD::Vec:unit (XD::Vec:vxv Vz Vx))) ; Recompute orthogonal Y Axis
;; 计算从 UCS → WCS 的变换矩阵
(setq Rot(xd::mat:rotation3d '(0 0 0) (-(angle '(0 0 0) Vx)) '(0 0 1)))
(setq ang(xd::vec:angle Vz '(0 0 1.0)))
(setq Rot1 (xd::mat:rotation3d '(0 0 0) (- ang) Vx))
(setq Tr (XD::Mat:DispToMatrix
'((1. 0. 0. 0.) (0. 1. 0. 0.) (0. 0. 1. 0.) (0. 0. 0. 1.))
(mapcar '- Org)))
(setq m1 (xd::mat:m*m Rot1 Tr))
(setq m1 (xd::mat:m*m Rot m1))
;; 计算从 WCS → UCS 的变换矩阵
(setq Rot(xd::mat:rotation3d '(0 0 0) (angle '(0 0 0) Vx) '(0 0 1.0)))
(setq Rot1 (xd::mat:rotation3d '(0 0 0) ang Vx))
(setq Tr (XD::Mat:DispToMatrix
'((1. 0. 0. 0.) (0. 1. 0. 0.) (0. 0. 1. 0.) (0. 0. 0. 1.))
Org))
(setq m2 (xd::mat:m*m Tr Rot1))
(setq m2 (xd::mat:m*m m2 Rot))
(list m1 m2)
)
)
)
小弟在此谢过Marting大师！！！
高产 高效
页:
[1]