XD::Mat:AlignToWCS

marting

1. ;;; XD::Mat:AlignToWCS
   
2. ;;; 描述：
   
3. ;;;   根据给定的原点 Org 和两个方向向量 Vx、Vy，生成将自定义坐标系（UCS）与世界坐标系（WCS）对齐的变换矩阵。
   
4. ;;;   返回从 UCS → WCS 以及 WCS → UCS 的两个 4x4 仿射矩阵。
   
5. ;;;
   
6. ;;; 调用格式：
   
7. ;;;   (XD::Mat:AlignToWCS Org Vx Vy)
   
8. ;;;
   
9. ;;; 参数说明：
   
10. ;;;   Org - 原点（3D 点列表），如 (0 0 0)
    
11. ;;;   Vx  - X 轴方向向量（3D 向量列表）
    
12. ;;;   Vy  - Y 轴方向向量（3D 向量列表）
    
13. ;;;
    
14. ;;; 返回值：
    
15. ;;;   若 Vx 或 Vy 为零向量，返回单位矩阵；
    
16. ;;;   否则返回一个包含两个矩阵的列表：(list m1 m2)
    
17. ;;;   m1：UCS → WCS 变换矩阵
    
18. ;;;   m2：WCS → UCS 变换矩阵
    
19. 
    
20. (defun XD::Mat:AlignToWCS (Org Vx Vy / Vz Rot ang Rot1 Tr m1 m2)
    
21.   (if (or (equal Vx '(0 0 0) 1e-14) (equal Vy '(0 0 0) 1e-14))
    
22.     '((1. 0. 0. 0.) (0. 1. 0. 0.) (0. 0. 1. 0.) (0. 0. 0. 1.))
    
23.     (progn
    
24.       (setq Vx  (XD::Vec:unit Vx))                          ; X Axis
    
25.       (setq Vy  (XD::Vec:unit Vy))                          ; Y Axis
    
26.       (setq Vz  (XD::Vec:unit (XD::Vec:vxv Vx Vy)))         ; Z Axis
    
27.       (setq Vy  (XD::Vec:unit (XD::Vec:vxv Vz Vx)))         ; Recompute orthogonal Y Axis
    
28. 
    
29.       ;; 计算从 UCS → WCS 的变换矩阵
    
30.       (setq Rot  (xd::mat:rotation3d '(0 0 0) (-(angle '(0 0 0) Vx)) '(0 0 1)))
    
31.       (setq ang  (xd::vec:angle Vz '(0 0 1.0)))
    
32.       (setq Rot1 (xd::mat:rotation3d '(0 0 0) (- ang) Vx))
    
33.       (setq Tr   (XD::Mat:DispToMatrix
    
34.                    '((1. 0. 0. 0.) (0. 1. 0. 0.) (0. 0. 1. 0.) (0. 0. 0. 1.))
    
35.                    (mapcar '- Org)))
    
36.       (setq m1   (xd::mat:m*m Rot1 Tr))
    
37.       (setq m1   (xd::mat:m*m Rot m1))
    
38. 
    
39.       ;; 计算从 WCS → UCS 的变换矩阵
    
40.       (setq Rot  (xd::mat:rotation3d '(0 0 0) (angle '(0 0 0) Vx) '(0 0 1.0)))
    
41.       (setq Rot1 (xd::mat:rotation3d '(0 0 0) ang Vx))
    
42.       (setq Tr   (XD::Mat:DispToMatrix
    
43.                    '((1. 0. 0. 0.) (0. 1. 0. 0.) (0. 0. 1. 0.) (0. 0. 0. 1.))
    
44.                    Org))
    
45.       (setq m2   (xd::mat:m*m Tr Rot1))
    
46.       (setq m2   (xd::mat:m*m m2 Rot))
    
47. 
    
48.       (list m1 m2)
    
49.     )
    
50.   )
    
51. )
    
52. 
    

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

sh_h

小弟在此谢过Marting大师！！！

杜阳

高产   高效