矩阵论

对CAD中矩阵及其相关知识的研究.
包括了线性代数的一些基本知识;
矩阵的基本运算;
矩阵的基本变换;
实体的矩阵及其变换;
方程求解和矩阵的求逆、矩阵的特征解;
等等。

下面是其实现的相关代码:

;|*********************************************************************************************;
软件作者: Highflybird                                                                          ;
软件用途: 为AutoCAD 的LISP定制的一些算法和函数                                                 ;
日期地点: 2012.12.12 深圳                                                                      ;
程序语言: AutoLISP,Visual LISP                                                                 ;
版本号:   Ver. 1.0.121212                                                                      ;
**********************************************************************************************|;
(setq MatLibSymbols
       '(MAT:v+v           MAT:v-v           MAT:v*v
         MAT:v/v           MAT:vxs           MAT:Dot
         MAT:vxv           MAT:SxVs          MAT:norm
         MAT:Norm3D        MAT:Unitization   MAT:unit
         MAT:Det2          MAT:Det3          MAT:Det2V
         MAT:Rot90         MAT:Rot2D         MAT:TransU2W
         MAT:TransW2U      MAT:trp           MAT:mxv
         MAT:mxp           MAT:mxs           MAT:m+m
         MAT:m-m           MAT:mxm           MAT:Translation
         MAT:TranslateBy2P MAT:Scaling       MAT:Rotation
         MAT:Rotation3D    MAT:RotateBy2P    MAT:Reflect
         MAT:Reflect3D     MAT:TransNested   MAT:RefGeom
         MAT:RevRefGeom    MAT:AttGeom       Mat:DispToMatrix
         MAT:Trans         MAT:u2w           MAT:w2u
         MAT:Align         MAT:2VMatrix      Mat:3PMatrix
         Mat:EntityMatrix  MAT:ISO           Mat:OcsMatrix
         MAT:TransList     Mat:3VLE          Mat:Detm
         Mat:3dPolarArray  Mat:Normal_Origin LM:ReflectByMatrix
         LM:Rotate3D       LM:Reflect3D      LM:TranslateByMatrix
         LM:RotateByMatrix LM:ScaleByMatrix  LM:ApplyMatrixTransformation
        )
)
;;;-----------------------------------------------------------;;
;;;符号保护                                                   ;;
;;;-----------------------------------------------------------;;
(defun protect-assign (syms)
  (eval (list 'pragma
              (list 'quote (list (cons 'protect-assign syms)))
        )
  )
)
;;;-----------------------------------------------------------;;
;;;符号解除保护                                               ;;
;;;-----------------------------------------------------------;;
(defun unprotect-assign (syms)
  (eval
    (list 'pragma
          (list 'quote (list (cons 'unprotect-assign syms)))
    )
  )
)
(unProtect-assign MatLibSymbols)
;;;***********************************************************;;
;;; 矩阵部分                                                  ;;
;;;***********************************************************;;
;;;-----------------------------------------------------------;;
;;; 两向量相加 addition                                       ;;
;;; Input: v1,v2 -vectors in R^n                              ;;
;;; OutPut: A vector                                          ;;
;;;-----------------------------------------------------------;;
(defun MAT:v+v (v1 v2)
  (mapcar '+ v1 v2)
)
;;;-----------------------------------------------------------;;
;;; 两向量相减  subtraction                                   ;;
;;; Input: v1,v2 -vectors in R^n                              ;;
;;; OutPut: A vector                                          ;;
;;;-----------------------------------------------------------;;
(defun MAT:v-v (v1 v2)
  (mapcar '- v1 v2)
)
;;;-----------------------------------------------------------;;
;;; 两向量相乘  multiplication                                ;;
;;; Input: v1,v2 -vectors in R^n                              ;;
;;; OutPut: A vector                                          ;;
;;;-----------------------------------------------------------;;
(defun MAT:v*v (v1 v2)
  (mapcar '* v1 v2)
)
;;;-----------------------------------------------------------;;
;;; 两向量相除  division                                      ;;
;;; Input: v1,v2 -vectors in R^n                              ;;
;;; OutPut: A vector                                          ;;
;;;-----------------------------------------------------------;;
(defun MAT:v/v (v1 v2)
  (mapcar '/ v1 v2)
)
;;;-----------------------------------------------------------;;
;;; 向量乘标量(系数)                                          ;;
;;; Vector x Scalar - Lee Mac                                 ;;
;;; Args: v - vector in R^n, s - real scalar                  ;;
;;;-----------------------------------------------------------;;
(defun MAT:vxs ( v s )
  (mapcar (function (lambda ( n ) (* n s))) v)
)
;;;-----------------------------------------------------------;;
;;; 向量除以标量(系数)                                        ;;
;;; Vector x Scalar - Lee Mac                                 ;;
;;; Args: v - vector in R^n, s - real scalar                  ;;
;;;-----------------------------------------------------------;;
(defun MAT:v/s ( v s )
  (if (not (zerop s))
    (mat:vxs v (/ 1.0 s))
  )
)
;;;-----------------------------------------------------------;;
;;; 两向量的点积                                              ;;
;;; Vector Dot Product                                        ;;
;;; Input: v1,v2 -vectors in R^n                              ;;
;;;-----------------------------------------------------------;;
(defun MAT:Dot (v1 v2)
  (apply '+ (mapcar '* v1 v2))
)
;;;-----------------------------------------------------------;;
;;; 两向量的叉积                                              ;;
;;; Vector Cross Product                                      ;;
;;; Args: u,v - vectors in R^3                                ;;
;;;-----------------------------------------------------------;;
(defun MAT:vxv ( u v )
  (list
    (- (* (cadr u) (caddr v)) (* (cadr v) (caddr u)))
    (- (* (car  v) (caddr u)) (* (car  u) (caddr v)))
    (- (* (car  u) (cadr  v)) (* (car  v) (cadr  u)))
  )
)
;;;-----------------------------------------------------------;;
;;; 线性组合  标量组乘向量组                                  ;;
;;; Linear combination - highflybird                          ;;
;;; Input: Vectors - vectors, Scalars, - a real number list   ;;
;;; Output: a vector                                          ;;
;;;-----------------------------------------------------------;;
(defun MAT:SxVs (Vectors Scalars)
  (apply 'mapcar (cons '+ (mapcar 'MAT:vxs Vectors Scalars)))
)
;;;-----------------------------------------------------------;;
;;; 向量的模(长度)                                            ;;
;;; Vector Norm - Lee Mac                                     ;;
;;; Args: v - vector in R^n                                   ;;
;;;-----------------------------------------------------------;;
(defun MAT:norm ( v )
  (sqrt (apply '+ (mapcar '* v v)))
)
;;;-----------------------------------------------------------;;
;;; 向量的模(长度)                                            ;;
;;; Vector Norm - highflybird                                 ;;
;;; Args: v - vector in R^3                                   ;;
;;;-----------------------------------------------------------;;
(defun MAT:Norm3D ( v )
  (distance '(0 0 0) v)
)
;;;-----------------------------------------------------------;;
;;; 单位向量                                                  ;;
;;; Unit Vector - Lee Mac                                     ;;
;;; Args: v - vector in R^n                                   ;;
;;;-----------------------------------------------------------;;
(defun MAT:Unitization (v)
  ( (lambda (n)
      (if (equal 0.0 n 1e-14)
        nil
        (MAT:vxs v (/ 1.0 n))
      )
    )
    (MAT:norm v)
  )
)
;;;-----------------------------------------------------------;;
;;; 单位向量                                                  ;;
;;; Unit Vector - highflybird                                 ;;
;;; Args: v - vector in R^3                                   ;;
;;;-----------------------------------------------------------;;
(defun MAT:unit ( v / l)
  (cond
    ( (= (setq l (distance '(0 0 0) v)) 1.0 ) v)
    ( (> l 1e-14) (mapcar '/ v (list l l l)))
  )
)                   
;;;-----------------------------------------------------------;;
;;; 两个2d向量的叉积的数值                                    ;;
;;; 输入: 两个点(或者两个向量)                              ;;
;;; 输出: 一个数值.如果为正则是逆时针,两向量形成的平面法线向量;;
;;;       向上,为负则是顺时针,为零则两向量共线或平行。      ;;
;;;       这个数值也为原点,P1,P2三点面积的两倍。              ;;
;;;-----------------------------------------------------------;;
(defun MAT:Det2V (v1 v2)
  (- (* (car v1) (cadr v2)) (* (car v2) (cadr v1)))
)
;;;-----------------------------------------------------------;;
;;; 2d行列式 determinant in R^2                               ;;
;;; Args: 4 numbers                                           ;;
;;;-----------------------------------------------------------;;
(defun MAT:Det2 (x1 y1 x2 y2)
  (- (* x1 y2) (* x2 y1))
)
;;;-----------------------------------------------------------;;
;;; 3d行列式  determinant in R^3                              ;;
;;; Args: 9 numbers                                           ;;
;;;-----------------------------------------------------------;;
(defun MAT:Det3 (a1 b1 c1 a2 b2 c2 a3 b3 c3)
  (+ (* a1 (- (* b2 c3) (* b3 c2)))
     (* a2 (- (* b3 c1) (* b1 c3)))
     (* a3 (- (* b1 c2) (* b2 c1)))
  )
)
;;;-----------------------------------------------------------;;
;;; n阶行列式  determinant in R^n                             ;;
;;; Args: n*n 矩阵                                            ;;
;;; Matrix Determinant  -  ElpanovEvgeniy                     ;;
;;; Last edit 2013.11.13                                      ;;
;;; Args: m - nxn matrix                                      ;;
;;; (mat:detm '((0 1) (1 0)))                                 ;;
;;;-----------------------------------------------------------;;
(defun mat:detm (m / r i)
  (cond
    ((null m) 1)
    ((and
       (zerop (setq i (caar m)))
       (setq r (car (vl-member-if-not
                      (function (lambda (a) (zerop (car a))))
                      (cdr m)
                    )
               )
       )
     )
     (mat:detm (cons (mapcar '+ (car m) r) (cdr m)))
    )
    ((zerop i) 0)
    ((setq i (float i))
     (setq r (cdar m))
     (* i
        (mat:detm
          (mapcar
            (function
              (lambda (a / d)
                (setq d (/ (car a) i))
                (mapcar
                  (function (lambda (b c) (- b (* c d))))
                  (cdr a)
                  r
                )
              )
            )
            (cdr m)
          )
        )
      )
    )
  )
)
;;;=====================================================
;;;三元一次方程组的解
;;;The solution of a Three-variable linear equations
;;;=====================================================
(defun Mat:3VLE (a b c d e f g h i j k l / d1)
  (setq d1 (float (Mat:DET3 a b c d e f g h i)))
  (if (/= d1 0.0)
    (list
      (/ (Mat:DET3 j b c k e f l h i) d1)
      (/ (Mat:DET3 a j c d k f g l i) d1)
      (/ (Mat:DET3 a b j d e k g h l) d1)
    )
  )
)
;;;-----------------------------------------------------------;;
;;; 旋转一个向量或者点90度                                    ;;
;;; 输入: 一个向量                                            ;;
;;; 输出: 被旋转90度后的向量                                  ;;
;;;-----------------------------------------------------------;;
(defun MAT:Rot90 (vec)
  (vl-list* (- (cadr vec)) (car vec) (cddr vec))
)
;;;-----------------------------------------------------------;;
;;; 旋转向量到指定角度                                        ;;
;;; 输入: 一个向量和指定的角度                                ;;
;;; 输出: 被旋转后的向量                                      ;;
;;;-----------------------------------------------------------;;
(defun MAT:Rot2D (v a / c s x y)
  (setq c (cos a) s (sin a))
  (setq x (car v) y (cadr v))
  (list (- (* x c) (* y s)) (+ (* x s) (* y c)))
)
;;;-----------------------------------------------------------;;
;;; 矩阵转置                                                  ;;
;;; MAT:trp Transpose a matrix -Doug Wilson-                  ;;
;;; 输入:矩阵                                                ;;
;;; 输出:转置后的矩阵                                        ;;
;;;-----------------------------------------------------------;;
(defun MAT:trp (m)
  (apply 'mapcar (cons 'list m))
)
;;;-----------------------------------------------------------;;
;;; 矩阵相加                                                  ;;
;;; Matrix + Matrix - Lee Mac                                 ;;
;;; Args: m,n - nxn matrices                                  ;;
;;;-----------------------------------------------------------;;
(defun MAT:m+m ( m n )
  (mapcar '(lambda ( r s ) (mapcar '+ r s)) m n)
)
;;;-----------------------------------------------------------;;
;;; 矩阵相减                                                  ;;
;;; Matrix - Matrix - Lee Mac                                 ;;
;;; Args: m,n - nxn matrices                                  ;;
;;;-----------------------------------------------------------;;
(defun MAT:m-m ( m n )
  (mapcar '(lambda ( r s ) (mapcar '- r s)) m n)
)
;;;-----------------------------------------------------------;;
;;; 矩阵相乘                                                  ;;
;;; MAT:mxm Multiply two matrices -Vladimir Nesterovsky-      ;;
;;;-----------------------------------------------------------;;
(defun MAT:mxm (m q)
  (mapcar (function (lambda (r) (MAT:mxv (MAT:trp q) r))) m)
)
;;;-----------------------------------------------------------;;
;;; 向量或点的矩阵变换(向量乘矩阵)                            ;;
;;; Matrix x Vector - Vladimir Nesterovsky                    ;;
;;; Args: m - nxn matrix, v - vector in R^n                   ;;
;;;-----------------------------------------------------------;;
(defun MAT:mxv (m v)
  (mapcar (function (lambda (r) (apply '+ (mapcar '* r v)))) m)
)
;;;-----------------------------------------------------------;;
;;; 点的矩阵(4x4 matrix) 变换                                 ;;
;;; 输入:矩阵m和一个三维点p                                  ;;
;;; 输出:点变换后的位置                                      ;;
;;;-----------------------------------------------------------;;
(defun MAT:mxp (m p)
  (reverse (cdr (reverse (MAT:mxv m (append p '(1.0))))))
)
;;;-----------------------------------------------------------;;
;;; 矩阵乘标量                                                ;;
;;; Matrix x Scalar - Lee Mac                                 ;;
;;; Args: m - nxn matrix, n - real scalar                     ;;
;;;-----------------------------------------------------------;;
(defun MAT:mxs ( m s )
  (mapcar (function (lambda ( v )(MAT:VxS v s))) m)
)
;;;-----------------------------------------------------------;;
;;;高斯消元法之矩阵分解                                       ;;
;;;Mat:Gauss_Elimination, use Safearray to express a matrix   ;;
;;;-----------------------------------------------------------;;
(defun Mat:Gauss_Elimination (m / r v i a)
  (if (car m)
    (progn
      (setq v (mapcar (function (lambda (x) (abs (car x)))) m)) ;首列的绝对值
      (setq i (vl-position (apply 'max v) v))                   ;其最大值所在的行号
      (setq r (nth i m))                                        ;得到绝对值的最大值所在的行
      (setq a (float (car r)))                                  ;需转化为浮点数,防止整除
      (if (equal a 0 1e-14)
        (Mat:Gauss_Elimination (mapcar 'cdr m))                 ;去掉全部为零的列
        (progn
          (setq m (vl-remove r m))                              ;去掉相同的行
          (setq r (mat:vxs r (/ 1.0 a)))                        ;归一化
          (setq m
            (mapcar
              (function
                (lambda (z)
                  (mapcar
                    (function
                      (lambda (x y)
                        (- x (* (car z) y))                     ;消元
                      )
                    )
                    z
                    r
                  )
                )
              )
              m
            )
          )
          (cons r (Mat:Gauss_Elimination (mapcar 'cdr m)))
        )
      )
    )
  )
)
;;;-----------------------------------------------------------;;
;;;LU三角形分解,没考虑除零情况                               ;;
;;;-----------------------------------------------------------;;
(defun Mat:LUDcmp (mat / A I J K M N SUM)
  (setq m (length mat))
  (setq n (length (car mat)))
  (setq a (vlax-make-safearray 5 (cons 0 (1- m)) (cons 0 (1- n))))
  (vlax-safearray-fill a mat)
  (setq j 0)
  (repeat n
    (setq i 0)
    (repeat m
      (if (<= i j)
        (progn
          (setq sum 0)
          (setq k 0)
          (repeat i
            (setq sum (+ sum (* (vlax-safearray-get-element a i k) (vlax-safearray-get-element a k j))))
            (setq k (1+ k))
          )
          (vlax-safearray-put-element a i j (- (vlax-safearray-get-element a i j) sum))
        )
        (progn
          (setq sum 0)
          (setq k 0)
          (repeat j
            (setq sum (+ sum (* (vlax-safearray-get-element a i k) (vlax-safearray-get-element a k j))))
            (setq k (1+ k))
          )
          (vlax-safearray-put-element a i j
            (/ (- (vlax-safearray-get-element a i j) sum) (float (vlax-safearray-get-element a j j)))
          )
        )
      )
      (setq i (1+ i))
    )
    (setq j (1+ j))
  )
  (vlax-safearray->list a)
)
;;;-----------------------------------------------------------;;
;;;从高斯消元法得到的三角形矩阵回代解方程                     ;;
;;;-----------------------------------------------------------;;
(defun Mat:TriangularForm (m / I L R X Y Z)
  (if (= (- (length (car m)) (length m)) 1)
    (progn
      (setq m (mapcar 'reverse (reverse m)))
      (setq i 1)
      (setq L (cons (caar m) L))
      (while (setq m (cdr m))
        (setq r (car m))
        (setq y 0)
        (setq z (reverse L))
        (repeat i
          (setq r (cdr r))
          (setq x (car r))
          (setq y (+ y (* x (car z))))
          (setq z (cdr z))
        )
        (setq y (- (caar m) y))
        (setq L (cons y L))
        (setq i (1+ i))
      )
      L
    )
  )
)
;;;-----------------------------------------------------------;;
;;;解一次方程组                                               ;;
;;;-----------------------------------------------------------;;
(defun Mat:Gauss_Equations (mat)
  (Mat:TriangularForm (Mat:Gauss_Elimination mat))
)
;;;-----------------------------------------------------------;;
;;;验证解                                                     ;;
;;;-----------------------------------------------------------;;
(defun Mat:Verify (mat ans fuzz)
  (equal (mat:mxv mat ans) (mapcar 'last mat) fuzz)
)
;;;-----------------------------------------------------------;;
;;;随机函数                                                   ;;
;;;-----------------------------------------------------------;;
(defun Misc:Rand (nMin nMax / seed)
  (setq seed (getvar "USERR4"))
  (if (= seed 0.)
    (setq seed (getvar "TDUSRTIMER")
          seed (- seed (fix seed))
          seed (rem (* seed 86400) 1)
    )
  )
  (setq seed (rem (+ (* seed 15625.7) 0.21137152) 1))
  (setvar "USERR4" seed)
  (+ nMin (* seed (- nMax nMin)))
)
;;;-----------------------------------------------------------;;
;;;随机矩阵                                                   ;;
;;;-----------------------------------------------------------;;
(defun Mat:RandomMatrix (len imax / m n)
  (repeat len
    (setq n nil)
    (repeat (1+ len)
      (setq n (cons (Misc:Rand (- imax) imax) n))
    )
    (setq m (cons n m))
  )
)
;;;***********************************************************;;
;;;矩阵之变换部分                                             ;;
;;;***********************************************************;;
;;;-----------------------------------------------------------;;
;;; 平移变换矩阵方式1                                         ;;
;;; 参数:                                                     ;;
;;; v - 位移矢量                                              ;;
;;;-----------------------------------------------------------;;
;;;---------------=={ Translate by Matrix }==-----------------;;
;;;                                                           ;;
;;; Translation Matrix                                        ;;
;;;-----------------------------------------------------------;;
;;; Author: highflybird, Copyright ? 2012                     ;;
;;;-----------------------------------------------------------;;
;;; Arguments:                                                ;;
;;; v  - Displacement vector by which to translate            ;;
;;;-----------------------------------------------------------;;
(defun MAT:Translation ( v )
  (list
    (list 1. 0. 0. (car v))
    (list 0. 1. 0. (cadr v))
    (list 0. 0. 1. (caddr v))
    (list 0. 0. 0. 1.)
  )
)
;;;-----------------------------------------------------------;;
;;; 平移变换矩阵方式2                                         ;;
;;; 参数:                                                     ;;
;;; p1 - 基点                                                 ;;
;;; p2 - 目标点                                               ;;
;;;-----------------------------------------------------------;;
;;;---------------=={ Translate by Matrix }==-----------------;;
;;;                                                           ;;
;;; Translation Matrix                                        ;;
;;;-----------------------------------------------------------;;
;;; Author: highflybird, Copyright ? 2012                     ;;
;;;-----------------------------------------------------------;;
;;; Arguments:                                                ;;
;;; p1, p2 - Points representing vector by which to translate ;;
;;;-----------------------------------------------------------;;
(defun MAT:TranslateBy2P ( p1 p2 )
  (MAT:Translation (mapcar '- p2 p1))
)
;;;-----------------------------------------------------------;;
;;; 比例缩放矩阵                                              ;;
;;; 参数:                                                     ;;
;;; Cen   - 基点                                              ;;
;;; scale - 缩放比例                                          ;;
;;;-----------------------------------------------------------;;
;;;-----------------=={ Scale by Matrix }==-------------------;;
;;;                                                           ;;
;;; Scaling Matrix                                            ;;
;;;-----------------------------------------------------------;;
;;; Author: highflybird, Copyright ? 2012                     ;;
;;;-----------------------------------------------------------;;
;;; Arguments:                                                ;;
;;; Cen    - Base Point for Scaling Transformation            ;;
;;; scale  - Scale Factor by which to scale object            ;;
;;;-----------------------------------------------------------;;
(defun MAT:Scaling ( Cen scale / s)
  (setq s (- 1 scale))
  (list
    (list scale 0. 0. (* s (car Cen)))
    (list 0. scale 0. (* s (cadr Cen)))
    (list 0. 0. scale (* s (caddr Cen)))
    '(0. 0. 0. 1.)
  )
)
;;;-----------------------------------------------------------;;
;;; 二维旋转变换矩阵                                          ;;
;;; 参数:                                                     ;;
;;; Cen - 基点                                                ;;
;;; ang - 旋转角度                                            ;;
;;;-----------------------------------------------------------;;
;;;-----------------=={ Rotate by Matrix }==------------------;;
;;;                                                           ;;
;;; Rotation Matrix                                           ;;
;;;-----------------------------------------------------------;;
;;; Author: highflybird, Copyright ? 2012                     ;;
;;;-----------------------------------------------------------;;
;;; Arguments:                                                ;;
;;; Cen    - Base Point for Rotation Transformation           ;;
;;; ang    - Angle through which to rotate object             ;;
;;;-----------------------------------------------------------;;
(defun MAT:Rotation ( Cen ang / c s x y)
  (setq c (cos ang) s (sin ang))
  (setq x (car Cen) y (cadr Cen))
  (list
    (list c (- s) 0. (- x (- (* c x) (* s y))))
    (list s    c  0. (- y (+ (* s x) (* c y))))
    '(0. 0. 1. 0.)
    '(0. 0. 0. 1.)
  )
)
;;;-----------------------------------------------------------;;
;;; 三维旋转变换矩阵                                          ;;
;;; 参数:                                                     ;;
;;; Cen  - 基点                                               ;;
;;; Axis - 旋转轴                                             ;;
;;; ang  - 旋转角                                             ;;
;;;-----------------------------------------------------------;;
;;;---------------=={ 3D Rotate by Matrix }==-----------------;;
;;; Author: highflybird.                                      ;;
;;; Arguments:                                                ;;
;;; Cen ---Input origin point of rotation                     ;;
;;; Axis---Input axis vector of rotation                      ;;
;;; Ang ---Input angle of rotation                            ;;
;;;-----------------------------------------------------------;;
(defun MAT:Rotation3D (Cen Axis Ang / A B C D M N P x y z)
  (setq D (distance '(0 0 0) Axis))
  (if (or (< D 1e-8) (zerop ang))
    '((1. 0. 0. 0.) (0. 1. 0. 0.) (0. 0. 1. 0.) (0. 0. 0. 1.))
    (setq N (mapcar '/ Axis (list D D D))
          x (car N)
          y (cadr N)
          z (caddr N)
          A (cos Ang)
          B (sin Ang)
          C (- 1 A)
          M (list (list (+ A (* x x C))
                        (- (* x y C) (* z B))
                        (+ (* y B) (* x z C))
                  )
                  (list (+ (* z B) (* x y C))
                        (+ A (* y y C))
                        (- (* y z C) (* x B))
                  )
                  (list (- (* x z C) (* y B))
                        (+ (* x B) (* y z C))
                        (+ A (* z z C))
                  )
            )
          p (mapcar '- Cen (Mat:mxv M Cen))
          M (Mat:DispToMatrix M p)
    )
  )
)
;;;-----------------------------------------------------------;;
;;; 三维旋转变换矩阵(通过两点和旋转角)                      ;;
;;; 参数:                                                     ;;
;;; p1,p2  - 两点定义的旋转轴                                 ;;
;;; ang    - 旋转角度                                         ;;
;;;-----------------------------------------------------------;;
;;;---------------=={ 3D Rotate by Matrix }==-----------------;;
;;; Rotation matrix                                           ;;
;;;-----------------------------------------------------------;;
;;; Author: highflybird, Copyright ? 2012                     ;;
;;;-----------------------------------------------------------;;
;;; Arguments:                                                ;;
;;; p1,p2  - Two 3D points defining the axis of rotation      ;;
;;; ang    - Rotation Angle                                   ;;
;;;-----------------------------------------------------------;;
(defun MAT:RotateBy2P ( p1 p2 ang )
   (MAT:Rotation3D P1 (mapcar '- p2 p1) ang)
)
;;;-----------------------------------------------------------;;
;;; 二维镜像变换矩阵                                          ;;
;;; 参数:                                                     ;;
;;; p1     - 镜像向量第一点                                   ;;
;;; p2     - 镜像向量第二点                                   ;;
;;;-----------------------------------------------------------;;
;;;----------------=={ Reflect by Matrix }==------------------;;
;;;                                                           ;;
;;; Reflects a VLA-Object or Point List using a               ;;
;;; Transformation Matrix                                     ;;
;;;-----------------------------------------------------------;;
;;; Author: Lee Mac, Copyright ? 2010 - www.lee-mac.com       ;;
;;;-----------------------------------------------------------;;
;;; Arguments:                                                ;;
;;; target - VLA-Object or Point List to transform            ;;
;;; p1, p2 - Points representing vector in which to reflect   ;;
;;;-----------------------------------------------------------;;
(defun MAT:Reflect ( p1 p2 / a c s x y)
  (setq a (angle p1 p2) a (+ a a))
  (setq c (cos a) s (sin a))
  (setq x (car p1) y (cadr p1))
  (list
    (list c    s  0. (- x (+ (* c x) (* s y))))
    (list s (- c) 0. (- y (- (* s x) (* c y))))
    '(0. 0. 1. 0.)
    '(0. 0. 0. 1.)
  )
)
;;;-----------------------------------------------------------;;
;;; 三维镜像变换矩阵                                          ;;
;;; 参数:                                                     ;;
;;; p1,p2,p3 - 三点定义的镜像平面                             ;;
;;;-----------------------------------------------------------;;
;;;---------------=={ 3D Reflect by Matrix }==----------------;;
;;;                                                           ;;
;;; Reflection matrix                                         ;;
;;;-----------------------------------------------------------;;
;;; Author: highflybird, Copyright ? 2012-                    ;;
;;;-----------------------------------------------------------;;
;;; Arguments:                                                ;;
;;; p1,p2,p3 - Three 3D points defining the reflection plane  ;;
;;;-----------------------------------------------------------;;
(defun MAT:Reflect3D (p1 p2 p3 / m ux uy uz)
  (mapcar
    'set
    '(ux uy uz)
    (MAT:unit (MAT:vxv (mapcar '- p2 p1) (mapcar '- p3 p1)))
  )
  (setq m (list (list (- 1. (* 2. ux ux)) (* -2. uy ux) (* -2. ux uz))
                (list (* -2. ux uy) (- 1. (* 2. uy uy)) (* -2. uy uz))
                (list (* -2. ux uz) (* -2. uy uz) (- 1. (* 2. uz uz)))
          )
  )
  (Mat:DispToMatrix m (mapcar '- p1 (MAT:mxv m p1)))
)
;;;***********************************************************;;
;;; 以下部分来自Lee-Mac,特地致谢!                             ;;
;;;***********************************************************;;
;;;---------------=={      二维变换       }==-----------------;;
;;;-----------------------------------------------------------;;
;;;-----------------------------------------------------------;;
;;; 比例缩放矩阵                                              ;;
;;; 参数:                                                     ;;
;;; target - vla-object 或者点                                ;;
;;; p1     - 基点                                             ;;
;;; scale  - 缩放比例                                         ;;
;;;-----------------------------------------------------------;;
;;;-----------------=={ Scale by Matrix }==-------------------;;
;;;                                                           ;;
;;; Scales a VLA-Object or Point List using a                 ;;
;;; Transformation Matrix                                     ;;
;;;-----------------------------------------------------------;;
;;; Author: Lee Mac, Copyright ? 2010 - www.lee-mac.com       ;;
;;;-----------------------------------------------------------;;
;;; Arguments:                                                ;;
;;; target - VLA-Object or Point List to transform            ;;
;;; p1     - Base Point for Scaling Transformation            ;;
;;; scale  - Scale Factor by which to scale object            ;;
;;;-----------------------------------------------------------;;
(defun LM:ScaleByMatrix ( target p1 scale / m )
  (LM:ApplyMatrixTransformation target
    (setq m
      (list
        (list scale 0. 0.)
        (list 0. scale 0.)
        (list 0. 0. scale)
      )
    )
    (mapcar '- p1 (MAT:mxv m p1))
  )
)
;;;-----------------------------------------------------------;;
;;; 平移变换矩阵                                              ;;
;;; 参数:                                                     ;;
;;; target - vla-object 或者点                                ;;
;;; p1     - 基点                                             ;;
;;; p2     - 目标点                                           ;;
;;;-----------------------------------------------------------;;
;;;---------------=={ Translate by Matrix }==-----------------;;
;;;                                                           ;;
;;; Translates a VLA-Object or Point List using a             ;;
;;; Transformation Matrix                                     ;;
;;;-----------------------------------------------------------;;
;;; Author: Lee Mac, Copyright ? 2010 - www.lee-mac.com       ;;
;;;-----------------------------------------------------------;;
;;; Arguments:                                                ;;
;;; target - VLA-Object or Point List to transform            ;;
;;; p1, p2 - Points representing vector by which to translate ;;
;;;-----------------------------------------------------------;;
(defun LM:TranslateByMatrix ( target p1 p2 )
  (LM:ApplyMatrixTransformation target
    (list
      (list 1. 0. 0.)
      (list 0. 1. 0.)
      (list 0. 0. 1.)
    )
    (mapcar '- p2 p1)
  )
)
;;;-----------------------------------------------------------;;
;;; 旋转变换矩阵                                              ;;
;;; 参数:                                                     ;;
;;; target - vla-object 或者点                                ;;
;;; p1     - 基点                                             ;;
;;; ang    - 旋转角度                                         ;;
;;;-----------------------------------------------------------;;
;;;-----------------=={ Rotate by Matrix }==------------------;;
;;;                                                           ;;
;;; Rotates a VLA-Object or Point List using a                ;;
;;; Transformation Matrix                                     ;;
;;;-----------------------------------------------------------;;
;;; Author: Lee Mac, Copyright ? 2010 - www.lee-mac.com       ;;
;;;-----------------------------------------------------------;;
;;; Arguments:                                                ;;
;;; target - VLA-Object or Point List to transform            ;;
;;; p1     - Base Point for Rotation Transformation           ;;
;;; ang    - Angle through which to rotate object             ;;
;;;-----------------------------------------------------------;;
(defun LM:RotateByMatrix ( target p1 ang / m)
  (LM:ApplyMatrixTransformation target
    (setq m
      (list
        (list (cos ang) (- (sin ang)) 0.)
        (list (sin ang)    (cos ang)  0.)
        (list    0.           0.      1.)
      )
    )
    (mapcar '- p1 (MAT:mxv m p1))
  )
)
;;;-----------------------------------------------------------;;
;;; 镜像变换矩阵                                              ;;
;;; 参数:                                                     ;;
;;; target - vla-object 或者点                                ;;
;;; p1     - 镜像向量第一点                                   ;;
;;; p2     - 镜像向量第二点                                   ;;
;;;-----------------------------------------------------------;;
;;;----------------=={ Reflect by Matrix }==------------------;;
;;;                                                           ;;
;;; Reflects a VLA-Object or Point List using a               ;;
;;; Transformation Matrix                                     ;;
;;;-----------------------------------------------------------;;
;;; Author: Lee Mac, Copyright ? 2010 - www.lee-mac.com       ;;
;;;-----------------------------------------------------------;;
;;; Arguments:                                                ;;
;;; target - VLA-Object or Point List to transform            ;;
;;; p1, p2 - Points representing vector in which to reflect   ;;
;;;-----------------------------------------------------------;;
(defun LM:ReflectByMatrix ( target p1 p2 / m)
  (
    (lambda ( a / m )
      (LM:ApplyMatrixTransformation target
        (setq m
          (list
            (list (cos a)    (sin a)  0.)
            (list (sin a) (- (cos a)) 0.)
            (list    0.         0.    1.)
          )
        )
        (mapcar '- p1 (MAT:mxv m p1))
      )
    )
    (* 2. (angle p1 p2))
  )
)
;;;-----------------------------------------------------------;;
;;; 变换函数                                                  ;;
;;; 参数:                                                     ;;
;;; target - vla-object 或者点                                ;;
;;; matrix - 3x3 矩阵                                         ;;
;;; vector - 移动向量                                         ;;
;;;-----------------------------------------------------------;;
;;;----------=={ Apply Matrix Transformation }==--------------;;
;;;                                                           ;;
;;; Transforms a VLA-Object or Point List using a             ;;
;;; Transformation Matrix                                     ;;
;;;-----------------------------------------------------------;;
;;; Author: Lee Mac, Copyright ? 2010 - www.lee-mac.com       ;;
;;;-----------------------------------------------------------;;
;;; Arguments:                                                ;;
;;; target - VLA-Object or Point List to Transform            ;;
;;; matrix - 3x3 Matrix by which to Transform object          ;;
;;; vector - 3D translation vector                            ;;
;;;-----------------------------------------------------------;;
(defun LM:ApplyMatrixTransformation ( target matrix vector )
  (cond
    ( (eq 'VLA-OBJECT (type target))
      (vla-TransformBy target
        (vlax-tMatrix
          (append (mapcar (function (lambda ( x v ) (append x (list v)))) matrix vector)
           '((0. 0. 0. 1.))
          )
        )
      )
    )
    ( (listp target)
      (mapcar
        (function
          (lambda ( point ) (mapcar '+ (MAT:mxv matrix point) vector))
        )
        target
      )
    )
  )
)
;;;---------------=={      三维变换       }==-----------------;;
;;;-----------------------------------------------------------;;
;;;-----------------------------------------------------------;;
;;; 三维旋转变换矩阵                                          ;;
;;; 参数:                                                     ;;
;;; target - vla-object 或者点                                ;;
;;; p1,p2  - 两点定义的旋转轴                                 ;;
;;; ang    - 旋转角度                                         ;;
;;;-----------------------------------------------------------;;
;;;---------------=={ 3D Rotate by Matrix }==-----------------;;
;;;                                                           ;;
;;; Rotates a VLA-Object or Point List about a 3D axis using  ;;
;;; a Transformation matrix.                                  ;;
;;;-----------------------------------------------------------;;
;;; Author: Lee Mac, Copyright ? 2011 - www.lee-mac.com       ;;
;;;-----------------------------------------------------------;;
;;; Arguments:                                                ;;
;;; target - VLA-Object or Point List to Rotate               ;;
;;; p1,p2  - Two 3D points defining the axis of rotation      ;;
;;; ang    - Rotation Angle                                   ;;
;;;-----------------------------------------------------------;;
(defun LM:Rotate3D ( target p1 p2 ang / ux uy uz u m)
  (mapcar 'set '(ux uy uz) (setq u (MAT:unit (mapcar '- p2 p1))))
  (LM:ApplyMatrixTransformation target
    (setq m
      (MAT:m+m
        (list
          (list (cos ang) 0. 0.)
          (list 0. (cos ang) 0.)
          (list 0. 0. (cos ang))
        )
        (MAT:m+m
          (MAT:mxs
            (list
              (list 0. (- uz) uy)
              (list uz 0. (- ux))
              (list (- uy) ux 0.)
            )
            (sin ang)
          )
          (MAT:mxs (mapcar (function (lambda ( e ) (MAT:vxs u e))) u) (- 1. (cos ang)))
        )
      )
    )
    (mapcar '- p1 (MAT:mxv m p1))
  )
)
;;;-----------------------------------------------------------;;
;;; 三维镜像变换矩阵                                          ;;
;;; 参数:                                                     ;;
;;; target   - vla-object 或者点                              ;;
;;; p1,p2,p3 - 三点定义的镜像平面                             ;;
;;;-----------------------------------------------------------;;
;;;---------------=={ 3D Reflect by Matrix }==----------------;;
;;;                                                           ;;
;;; Reflects a VLA-Object or Point List in a plane using a    ;;
;;; Transformation matrix.                                    ;;
;;;-----------------------------------------------------------;;
;;; Author: Lee Mac, Copyright ? 2011 - www.lee-mac.com       ;;
;;;-----------------------------------------------------------;;
;;; Arguments:                                                ;;
;;; target   - VLA-Object or Point List to Reflect            ;;
;;; p1,p2,p3 - Three 3D points defining the reflection plane  ;;
;;;-----------------------------------------------------------;;
(defun LM:Reflect3D ( target p1 p2 p3 / m u ux uy uz )
  (mapcar 'set '(ux uy uz) (setq u (MAT:unit (MAT:vxv (mapcar '- p2 p1) (mapcar '- p3 p1)))))
  (LM:ApplyMatrixTransformation target
    (setq m
      (list
        (list (- 1. (* 2. ux ux)) (* -2. uy ux) (* -2. ux uz))
        (list (* -2. ux uy) (- 1. (* 2. uy uy)) (* -2. uy uz))
        (list (* -2. ux uz) (* -2. uy uz) (- 1. (* 2. uz uz)))
      )
    )
    (mapcar '- p1 (MAT:mxv m p1))
  )
)
;;;-----------------------------------------------------------;;
;;; 变换函数                                                  ;;
;;; 参数:                                                     ;;
;;; target - vla-object 或者点                                ;;
;;; matrix - 3x3 矩阵                                         ;;
;;; vector - 移动向量                                         ;;
;;;-----------------------------------------------------------;;
;;;----------=={ Apply Matrix Transformation }==--------------;;
;;;                                                           ;;
;;; Transforms a VLA-Object or Point List using a             ;;
;;; Transformation Matrix                                     ;;
;;;-----------------------------------------------------------;;
;;; Author: Lee Mac, Copyright ? 2010 - www.lee-mac.com       ;;
;;;-----------------------------------------------------------;;
;;; Arguments:                                                ;;
;;; target - VLA-Object or Point List to Transform            ;;
;;; matrix - 3x3 Matrix by which to Transform object          ;;
;;; vector - 3D translation vector                            ;;
;;;-----------------------------------------------------------;;
(defun LM:ApplyMatrixTransformation ( target matrix vector )
  (cond
    ( (eq 'VLA-OBJECT (type target))
      (vla-TransformBy target
        (vlax-tMatrix
          (append (mapcar (function (lambda ( x v ) (append x (list v)))) matrix vector)
           '((0. 0. 0. 1.))
          )
        )
      )
    )
    ( (listp target)
      (mapcar
        (function
          (lambda ( point ) (mapcar '+ (MAT:mxv matrix point) vector))
        )
        target
      )
    )
  )
)
;;;-----------------------------------------------------------;;
;;; 块参照的变换矩阵和逆矩阵                                  ;;
;;;-----------------------------------------------------------;;
;;;-----------------------------------------------------------;;
;;; 功能: 某点在块内坐标系统和世界或者用户坐标系统的转换     ;;
;;; 参数: pt 要变换的点。                                    ;;
;;;        rlst 用 nentselp或者nentsel得到的表的最后一项      ;;
;;;        from  坐标系:0,WCS; 1,当前UCS; 2,块参照坐标系RCS  ;;
;;;        to    坐标系:0,WCS; 1,当前UCS; 2,块参照坐标系RCS  ;;
;;;-----------------------------------------------------------;;
;;; MAT:TransNested (gile)                                    ;;
;;; Translates a point coordinates from WCS or UCS to RCS     ;;
;;; -coordinates system of a                                  ;;
;;; reference (xref or block) whatever its nested level-      ;;
;;;                                                           ;;
;;; Arguments                                                 ;;
;;; pt : the point to translate                               ;;
;;; rlst : the parents entities list from the deepest nested  ;;
;;;        to the one inserted in current space -same as      ;;
;;;        (last (nentsel)) or (last (nentselp))              ;;
;;; from to : as with trans function: 0.WCS, 1.UCS, 2.RCS     ;;
;;;-----------------------------------------------------------;;
(defun MAT:TransNested (pt rlst from to / GEOM)
  (and (= 1 from) (setq pt (trans pt 1 0)))
  (and (= 2 to) (setq rlst (reverse rlst)))
  (and (or (= 2 from) (= 2 to))
       (while rlst
        (setq geom (if  (= 2 to)
                      (MAT:RevRefGeom (car rlst))
                      (MAT:RefGeom (car rlst))
                    )
               rlst (cdr rlst)
               pt   (mapcar '+ (MAT:mxv (car geom) pt) (cadr geom))
        )
       )
  )
  (if (= 1 to)
    (trans pt 0 1)
    pt
  )
)
;;;-----------------------------------------------------------;;
;;; 功能:图块的变换矩阵                                      ;;
;;; 输入:块参照的图元名                                      ;;
;;; 输出:块参照的变换矩阵                                    ;;
;;;-----------------------------------------------------------;;
;;; MAT:RefGeom (gile)                                        ;;
;;; Returns a list which first item is a 3x3 transformation   ;;
;;; matrix(rotation,scales normal) and second item the object ;;
;;; insertion point in its parent(xref, bloc or space)        ;;
;;;                                                           ;;
;;; Argument : an ename                                       ;;
;;;-----------------------------------------------------------;;
(defun MAT:RefGeom (ename / elst ang norm mat)
  (setq elst (entget ename)
        ang  (cdr (assoc 50 elst))
        norm (cdr (assoc 210 elst))
  )
  (list
    (setq mat
           (MAT:mxm
             (mapcar (function (lambda (v) (trans v 0 norm T)))
                     '((1.0 0.0 0.0) (0.0 1.0 0.0) (0.0 0.0 1.0))
             )
             (MAT:mxm
               (list (list (cos ang) (- (sin ang)) 0.0)
                     (list (sin ang) (cos ang) 0.0)
                     '(0.0 0.0 1.0)
               )
               (list (list (cdr (assoc 41 elst)) 0.0 0.0)
                     (list 0.0 (cdr (assoc 42 elst)) 0.0)
                     (list 0.0 0.0 (cdr (assoc 43 elst)))
               )
             )
           )
    )
    (mapcar
      '-
      (trans (cdr (assoc 10 elst)) norm 0)
      (MAT:mxv mat
           (cdr (assoc 10 (tblsearch "BLOCK" (cdr (assoc 2 elst)))))
      )
    )
  )
)
;;;-----------------------------------------------------------;;
;;; 功能:图块的变换矩阵的逆矩阵                              ;;
;;;-----------------------------------------------------------;;
;;; MAT:RevRefGeom (gile)                                     ;;
;;; MAT:RefGeom inverse function                              ;;
;;; 输入:块参照的图元名                                      ;;
;;; 输出:块参照的变换矩阵的逆矩阵                            ;;
;;;-----------------------------------------------------------;;
(defun MAT:RevRefGeom (ename / entData ang norm mat)
  (setq entData (entget ename)
        ang     (- (cdr (assoc 50 entData)))
        norm    (cdr (assoc 210 entData))
  )
  (list
    (setq mat
           (MAT:mxm
             (list (list (/ 1 (cdr (assoc 41 entData))) 0.0 0.0)
                   (list 0.0 (/ 1 (cdr (assoc 42 entData))) 0.0)
                   (list 0.0 0.0 (/ 1 (cdr (assoc 43 entData))))
             )
             (MAT:mxm
               (list (list (cos ang) (- (sin ang)) 0.0)
                     (list (sin ang) (cos ang) 0.0)
                     '(0.0 0.0 1.0)
               )
               (mapcar (function (lambda (v) (trans v norm 0 T)))
                       '((1.0 0.0 0.0) (0.0 1.0 0.0) (0.0 0.0 1.0))
               )
             )
           )
    )
    (mapcar '-
            (cdr (assoc 10 (tblsearch "BLOCK" (cdr (assoc 2 entData)))))
            (MAT:mxv mat (trans (cdr (assoc 10 entData)) norm 0))
    )
  )
)
;;;-----------------------------------------------------------;;
;;; 属性的变换矩阵Attrib Transformation Matrix. -highflybird  ;;
;;; 输入:Ename 属性的图元名                                  ;;
;;; 输出:属性的变换矩阵                                      ;;
;;;-----------------------------------------------------------;;
(defun MAT:AttGeom (ename / ang norm mat elst)
  (setq elst (entget ename)
        ang  (cdr (assoc 50 elst))
        norm (cdr (assoc 210 elst))
  )
  (list
    (setq mat
           (mxm
             (mapcar (function (lambda (v) (trans v 0 norm T)))
                     '((1.0 0.0 0.0) (0.0 1.0 0.0) (0.0 0.0 1.0))
             )
             (list (list (cos ang) (- (sin ang)) 0.0)
                   (list (sin ang) (cos ang) 0.0)
                   '(0.0 0.0 1.0)
             )
           )
    )
    (trans (cdr (assoc 10 elst)) norm 0)
  )
)
;;;-----------------------------------------------------------;;
;;; Append displacement vector to a matrix      -Highflybird- ;;
;;; 把位移矢量添加到矩阵中                                    ;;
;;; 输入:mat -- 矩阵(3x3),disp -- 位移矢量                  ;;
;;; 输出:一个4X4的变换CAD的标准变换矩阵                      ;;
;;;-----------------------------------------------------------;;
(defun Mat:DispToMatrix (mat disp)
  (append
    (mapcar 'append mat (mapcar 'list disp))
    '((0. 0. 0. 1.))
  )
)
;;;-----------------------------------------------------------;;
;;; 从一个坐标系统到另一个坐标系统的变换矩阵                  ;;
;;; 输入:from - 源坐标系;to - 目的坐标系                    ;;
;;; 输出:一个4X4的CAD变换矩阵                                ;;
;;;-----------------------------------------------------------;;
(defun MAT:Trans (from to)
  (append
    (MAT:trp
      (list
        (trans '(1 0 0) from to t)
        (trans '(0 1 0) from to t)
        (trans '(0 0 1) from to t)
        (trans '(0 0 0) from to nil)
      )
    )
    '((0. 0. 0. 1.))
  )
)
;;;-----------------------------------------------------------;;
;;; ucs到wcs矩阵,也可称UCS的变换矩阵                         ;;
;;;-----------------------------------------------------------;;
(defun MAT:u2w () (MAT:Trans 1 0))
;;;-----------------------------------------------------------;;
;;; wcs到ucs矩阵,也可称UCS的逆变换矩阵                       ;;
;;;-----------------------------------------------------------;;
(defun MAT:w2u () (MAT:Trans 0 1))
;;;-----------------------------------------------------------;;
;;OCS的变换矩阵,或叫法线矢量的变换矩阵                       ;;
;;;-----------------------------------------------------------;;
(defun Mat:OcsMatrix (zdir / xdir)
  (or (equal 1.0 (distance '(0 0 0) zdir) 1e-8)
    (setq zdir (Mat:Unit zdir))                                 ; 先把矢量单位化。
  )
  (if (and (< (abs (car zdir)) 0.015625)                        ; 如果(abs (Nx) < 1/64)
           (< (abs (cadr zdir)) 0.015625)                       ; 且 (abs (Ny) < 1/64)
      )
    (setq xdir (Mat:Unit (Mat:vxv '(0 1 0) zdir)))              ; Ax = Wy X N   (叉积)
    (setq xdir (Mat:Unit (Mat:vxv '(0 0 1) zdir)))              ;否则 Ax = Wz X N。
  )
  (list xdir (Mat:Unit (Mat:vxv zdir xdir)) zdir)               ;Y方向满足右手型坐标系统
)
;;;-----------------------------------------------------------;;
;;; 通用变换矩阵 by highflybird                               ;;
;;; 输入:from - 原坐标系,                                   ;;
;;;       to   - 目的坐标系,                                 ;;
;;;       Org  - 目的坐标系的原点相对原坐标系的位置           ;;
;;;       Ang  - 相对于原坐标系的旋转角度                     ;;
;;; 输出:两个矩阵,一个是从原坐标系变换到目的坐标系的变换矩阵;;
;;;       一个是从目的坐标系变换到原坐标系的变换矩阵          ;;
;;;-----------------------------------------------------------;;
(defun MAT:Align (from to Org Ang / Mat Rot Inv Cen)
  (setq Mat (mapcar (function (lambda (v) (trans v from to T)))
                    '((1. 0. 0.) (0. 1. 0.) (0. 0. 1.))
            )
  )
  (if (not (equal ang 0 1e-14))
    (setq Rot (list (list (cos ang) (- (sin ang)) 0.)
                    (list (sin ang) (cos ang) 0.)
                    (list 0. 0. 1.)
              )
          mat (MAT:mxm mat Rot)
    )
  )
  (setq Cen (trans Org to from))
  (setq Inv (mat:trp mat))
  (list
    (Mat:DispToMatrix mat Cen)                                  ;from->to
    (Mat:DispToMatrix Inv (mat:mxv mat (mapcar '- Cen)))        ;to->from
  )
)
;;;-----------------------------------------------------------;;
;;; 通过两个坐标轴和坐标原点定义的变换矩阵  -by highflybird   ;;
;;; 输入:Org  - 坐标系原点,                                 ;;
;;;       Vx   - 坐标系X 方向,                               ;;
;;;       Vy   - 坐标系y 方向                                 ;;
;;; 输出:两个矩阵,一个是该坐标系的变换矩阵,一个是其逆矩阵  ;;
;;;-----------------------------------------------------------;;
(defun MAT:2VMatrix (Org Vx Vy / Vz Rot)
  (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  (Mat:Unit Vx))                                  ;X Axis
      (setq Vy  (Mat:Unit Vy))                                  ;Y Axis
      (setq Vz  (Mat:unit (MAT:vxv Vx Vy)))                     ;Z Axis
      (setq Vy  (Mat:unit (MAT:vxv Vz Vx)))                     ;Y Axis
      (setq Rot (list Vx Vy Vz))                                ;Rotation matrix
      (list                                                     ;Inverse Rotation matrix
        (Mat:DispToMatrix (MAT:trp Rot) Org)                    ;The transformation matrix
        (Mat:DispToMatrix Rot (MAT:mxv Rot (mapcar '- Org)))    ;The Inverse matrix
      )
    )
  )
)
;;;-----------------------------------------------------------;;
;;; Mat:3PMatrix  -Highflybird-                               ;;
;;; 通过两个坐标轴和坐标原点定义的变换矩阵  -by highflybird   ;;
;;; 输入:P1 - 坐标系原点,                                   ;;
;;;       P2 - 坐标系的第2点                                  ;;
;;;       P3 - 坐标系的第3点                                  ;;
;;; 输出:两个矩阵,一个是该坐标系的变换矩阵,一个是其逆矩阵  ;;
;;;-----------------------------------------------------------;;
(defun Mat:3PMatrix (p1 p2 p3 / v1 v2 v3)
  (MAT:2VMatrix P1 (mapcar '- p2 p1) (mapcar '- p3 p1))
)
;;;-----------------------------------------------------------;;
;;; 平齐实体的变换矩阵  -by highflybird                       ;;
;;; 输入:Ent - 实体名                                        ;;
;;; 输出:平齐这个实体的变换矩阵和它的逆矩阵                  ;;
;;;-----------------------------------------------------------;;
(defun Mat:EntityMatrix (Ent / z dxf Cen obj an m1 mat Inv org)
  (setq dxf (entget ent))
  (if (setq Cen (cdr (assoc 10 dxf)))                           ;Insertpoint,center or startpoint,etc.
    (if (null (caddr Cen))
      (setq Cen (append Cen '(0.0)))
    )
    (setq Cen '(0 0 0))
  )
  (setq obj (vlax-ename->vla-object Ent))
  (if (and (vlax-property-available-p obj 'elevation)           ;If it has elevation value.
           (wcmatch (vla-get-objectname obj) "*Polyline")       ;It's a "AcDb2dPolyline" or "AcDbPolyline" object
      )
    (setq z   (vla-get-elevation obj)
          Cen (list (car Cen) (cadr Cen) (+ (caddr Cen) z))     ;add elevation value
    )
  )
  (if (vlax-property-available-p obj 'rotation)                 ;if it has a rotaion angle
    (setq an (vla-get-rotation obj))
    (setq an 0)
  )
  (MAT:Align 0 Ent Cen an)                                      ;return two matrices, the first is WCS->OCS,the second is OCS->WCS
)
;;;-----------------------------------------------------------;;
;;;通用的轴测变换矩阵     highflybird  2012.12                ;;
;;;Axonometric projections Rotation matrices                  ;;
;;;Isometric projection: a = (/ pi 4),b = (atan (- (sqrt 2))) ;;
;;;Input: a - Rotation angle about the vertical axis          ;;
;;;       b - Rotation angle about the horizontal axis        ;;
;;;Output: transforamtion matrix of this projection           ;;
;;;-----------------------------------------------------------;;
(defun MAT:ISO (a b / ca sa cb sb)
  (setq ca (cos a))
  (setq sa (sin a))
  (setq cb (cos b))
  (setq sb (sin b))
  (list (list ca        (- sa)    0      0)
        (list (* sa cb) (* ca cb) (- sb) 0)
        (list (* sa sb) (* ca sb) cb     0)
        (list 0 0 0 1)
  )
)
;;;-----------------------------------------------------------;;
;;; 点集变换                                                  ;;
;;; 输入: 要变换的点集                                        ;;
;;; 输出: 变换后的点集                                        ;;
;;;-----------------------------------------------------------;;
(defun MAT:TransList (points from to Disp)
  (mapcar (function (lambda (p) (trans p from to Disp))) points)
)
;;;-----------------------------------------------------------;;
;;; 点变换1                                                   ;;
;;; 输入: 要变换的点和原点及变换向量                          ;;
;;; 输出: 点变换后的位置                                      ;;
;;;-----------------------------------------------------------;;
(defun MAT:TransU2W (p p0 v / d x0 y0 x1 y1 dv rt)
  (setq d (distance '(0 0) v))
  (if (equal d 1e-14)
    P0
    (setq x1 (car  p)
          y1 (cadr p)
          x0 (car  v)
          y0 (cadr v)
          dv (list (/ (- (* x1 x0) (* y1 y0)) d)
                   (/ (+ (* y1 x0) (* x1 y0)) d)
             )
          rt (mapcar '+ P0 dv)
    )
  )
)
;;;-----------------------------------------------------------;;
;;; 点变换2                                                   ;;
;;; 输入: 要变换的点和原点及变换向量                          ;;
;;; 输出: 点变换后的位移向量                                  ;;
;;;-----------------------------------------------------------;;
(defun MAT:TransW2U (p p0 v / d x0 y0 x1 y1 dv)
  (setq d (distance '(0 0) v))
  (if (equal d 1e-14)
    (list 0 0)
    (setq x1 (- (car p) (car p0))
          y1 (- (cadr p) (cadr p0))
          x0 (car v)
          y0 (cadr v)
          dv (list (/ (+ (* x1 x0) (* y1 y0)) d)
                   (/ (- (* y1 x0) (* x1 y0)) d)
             )
    )
  )
)
;;;-----------------------------------------------------------;;
;;; 三维环形阵列                                              ;;
;;; 输入:Objlst    -- 物体集                                 ;;
;;;       Number    -- 要阵列的个数(包含自身在内)           ;;
;;;       FillAngle -- 旋转角度                               ;;
;;;       IsCCW     -- 是否逆时针                             ;;
;;;       P1        -- 阵列中心点                             ;;
;;;       P2        -- 阵列轴线的另一点                       ;;
;;; 输出:阵列的物体列表                                      ;;
;;;-----------------------------------------------------------;;
(defun Mat:3dPolarArray (Objlst Number FillAngle IsCCW P1 P2 / lst1 lst2 ANG MAT NEW)
  (if (and (= (type number) 'INT) (> number 1))
    (progn
      (if IsCCW
        (setq FillAngle (float FillAngle))
        (setq FillAngle (- FillAngle pi pi))
      )
      (setq ang (/ FillAngle (1- Number)))
      (setq mat (vlax-tmatrix (MAT:RotateBy2P P1 P2 ang)))
      (repeat (1- Number)
        (setq lst1 nil)
        (foreach obj ObjLst
          (setq new (vla-copy obj))
          (vla-transformby new mat)
          (setq lst1 (cons new lst1))
        )
        (setq objLst (reverse lst1))
        (setq lst2 (cons objLst lst2))
      )
      (reverse lst2)
    )
    (list ObjLst)
  )
)
;;;-----------------------------------------------------------;;
;;; 已知法线和原点的平面变换矩阵                              ;;
;;; 输入:Normal    -- 法线                                   ;;
;;;       Origin    -- 原点                                   ;;
;;; 输出:阵列的物体列表                                      ;;
;;;-----------------------------------------------------------;;
(defun Mat:Normal_Origin_1 (Normal Origin / mat)
  (setq mat (MAT:OCSMATRIX Normal))
  (list
    (Mat:DispToMatrix (mat:trp mat) origin)
    (mat:DispToMatrix mat (mat:mxv mat (mapcar '- Origin)))
  )
)
(defun Mat:Normal_Origin (Normal Origin / mat rev xdir ydir zdir)
  (setq xdir (trans '(1 0 0) 0 Normal T))
  (setq ydir (trans '(0 1 0) 0 Normal T))
  (setq zdir (trans '(0 0 1) 0 Normal T))
  (setq mat  (list xdir ydir zdir))
  (setq rev  (Mat:trp mat))
  (list
    (MAT:DISPTOMATRIX mat Origin)
    (MAT:DISPTOMATRIX rev (mat:mxv rev (mapcar '- Origin)))
  )
)
;;;-----------------------------------------------------------;;
;;; 选择集的包围盒                                            ;;
;;;-----------------------------------------------------------;;
(defun ENT:SelBox (sel / i ent obj MinPt MaxPt MinPts MaxPts objs)
  (setq i 0)
  (repeat (sslength sel)
    (setq ent (ssname sel i))
    (setq obj (vlax-ename->vla-object ent))
    (setq objs (cons obj objs))
    (vla-getboundingbox obj 'MinPt 'MaxPt)
    (setq MinPts (cons (vlax-safearray->list minPt) MinPts))
    (setq MaxPts (cons (vlax-safearray->list maxPt) MaxPts))
    (setq i (1+ i))
  )
  (list (reverse objs)
        (list (apply 'mapcar (cons 'min MinPts))
              (apply 'mapcar (cons 'max MaxPts))
        )
  )
)
;;;-----------------------------------------------------------;;
;;;获取物体Objects                                            ;;
;;;-----------------------------------------------------------;;
(defun Ent:Ents->Objs (sel / i e o l)
  (setq i (sslength sel))
  (repeat i
    (setq e (ssname sel (setq i (1- i))))
    (setq o (vlax-ename->vla-object e))
    (setq l (cons o l))
  )
)
;;;***********************************************************;;
;;; 矩阵测试部分                                              ;;
;;;***********************************************************;;
;|
Test for Mat:Normal_Origin
;;;法线原点矩阵测试
(defun c:tt ()
  (setq e (car (entsel)))
  (setq o (vlax-ename->vla-object e))
  (setq Normal (trans '(0 0 1) 1 0 T))
  (setq origin (getvar 'ucsorg))
  (command "undo" "be")
  (setq mat (mat:normal_origin Normal origin))
  (vla-transformby o (vlax-tmatrix (cadr mat)))
  (command "undo" "e")
  (Princ)
)
;;;3d环形阵列测试
(defun C:PolarArrayTest (/ ss N P1 P2 ActDoc ObjLst)
  (setq ss (ssget))
  (initget 7)
  (setq N (getInt "n数量:"))
  (initget 9)
  (setq P1 (getpoint "n中心点:"))
  (initget 9)
  (setq p2 (getpoint P1 "n另一点:"))
  (if ss
    (progn
      (setq ActDoc (vla-get-ActiveDocument (vlax-get-acad-object)))
      (vla-StartUndoMark ActDoc)
      (setq Ojblst (Ent:Ents->Objs ss))
      (Mat:3dPolarArray ojblst N (/ pi 2) nil (trans p1 1 0) (trans p2 1 0))
      (vla-EndUndoMark ActDoc)
      (vlax-release-object ActDoc)
      (princ)
    )
  )
)
;;;Align a 3dSolid to the WCS view
;;;轴测矩阵的测试
(defun C:Test (/ sel ent obj i an new MAT)
  (setq sel (ssget "_+.:E:S:L" '((0 . "3DSOLID"))))
  (if sel
    (progn
      (setq ent (ssname sel 0))
      (setq obj (vlax-ename->vla-object ent))
      (setq i 0)
      (setq an (atan (- (sqrt 2))))
      (foreach f '(0.25 0.75 1.25 1.75)                         ;Southwest,Northwest,Northeast,Southeast Isometric projection
        (setq mat (MAT:ISO (* f pi) an))
        (setq new (vla-copy obj))
        (vla-put-color new (setq i (1+ i)))
        (vla-transformby new (vlax-tmatrix mat))                ;transformate the object by matrix
      )
    )
  )
  (princ)
)
;;;一些矩阵函数的测试
(defun c:ccc (/ DXF E ENT I INS MAT0 MAT1 MAT2 MAT3 MAT4 MAT5 MAT6 MAT7 MAT8 MAT9 O ORG SEL VX VY)
  (if (setq ent (car (entsel "n要平齐的对象:")))               ;(setq sel (ssget ":S" '((0 . "CIRCLE"))))
    (progn
      (setq dxf  (entget ent))
      (setq ins  (cdr (assoc 10 dxf)))
      (setq vx   (getvar 'ucsxdir))
      (setq vy   (getvar 'ucsydir))
      (setq org  (getvar 'ucsorg))
      (setq Mat0 (Mat:EntityMatrix ent))
      (setq mat1 (cadr mat0))                                   ;OCS->WCS
      (setq mat0 (car mat0))                                    ;WCS->OCS(trans Pt WCS OCS)
      (setq mat2 (Mat:u2w))                                     ;UCS的变换矩阵
      (setq mat3 (Mat:w2u))                                     ;UCS的变换矩阵的逆矩阵
      (setq Mat4 (MAT:2VMatrix org vx vy))                      ;UCS的变换矩阵
      (setq mat5 (cadr mat4))                                   ;WCS->UCS
      (setq mat4 (car mat4))                                    ;UCS->WCS(trans Pt UCS WCS)
      (setq mat6 (Mat:trans 1 0))                               ;UCS的变换矩阵
      (setq mat7 (mat:trans 0 1))                               ;UCS的变换矩阵的逆矩阵
      (setq i -1)
      (if (setq sel (ssget))
        (progn
          (command "undo" "be")
          (repeat (sslength sel)
            (setq e (ssname sel (setq i (1+ i))))
            (setq o (vlax-ename->vla-object e))
            (vla-transformby o (vlax-tmatrix mat0))
          )
          (command "undo" "e")
        )
      )
    )
  )
  (princ)
)
;;;比例缩放矩阵的测试
(defun c:test1 (/ ENT I MAT OBJ PT SC SS)
  (initget 1)
  (setq Pt (getpoint "n比例缩放基点:"))
  (initget 7)
  (setq sc (getreal "n缩放倍数:"))
  (setq mat (MAT:Trans 0 (list 0 0 sc)))
  (setq mat (vlax-tmatrix mat))
  (setq i -1)
  (if (setq ss (ssget))
    (repeat (sslength ss)
      (setq ent (ssname ss (setq i (1+ i))))
      (setq obj (vlax-ename->vla-object ent))
      (MAT:ScaleByMatrix obj mat)
    )
  )
)
;;;镜像,旋转和3点矩阵的测试
(defun c:test2 (/ AN ENT I MAT OBJ P0 P1 P2 SS)
  (initget 1)
  (setq P1 (getpoint "n 点1:"))
  (initget 1)
  (setq P2 (getpoint p1 "n 点2:"))
  ;(initget 1)
  ;(setq P3 (getpoint p1 "n 点2:"))
  (grdraw p1 p2 1)
  ;(grdraw p2 p3 1)
  ;(grdraw p3 p1 1)
  (setq P1 (trans p1 1 0))
  (setq P2 (trans p2 1 0))
  ;(setq P3 (trans p3 1 0))
  (initget 1)
  (setq an (getangle "n旋转角度:"))
  (initget 7)
  (setq sc (getreal "n缩放倍数:")) 
  (setq p0 '(2.3 1.3 -1.2))
;;;  (setq s
;;;  (Misc:test 10000
;;;    '(
;;;      (MAT:Reflect p1 p2)
;;;    )
;;;  ))
  (setq mat (MAT:RotateBy2P P1 P2 an))
  (setq mat (vlax-tmatrix mat))
  (setq i -1)
  (command "undo" "be")
  (if (setq ss (ssget))
    (repeat (sslength ss)
      (setq ent (ssname ss (setq i (1+ i))))
      (setq obj (vlax-ename->vla-object ent))
      (setq obj (vla-copy obj))
      (vla-transformby obj mat)
    )
  )
  (command "undo" "e")
  (princ)
)
;;;-----------------------------------------------------------;;
;;;镜像,旋转和缩放的变换矩阵的测试                           ;;
;;;-----------------------------------------------------------;;
;;;以下例子演示:                                             ;;
;;;把选择集的所有物体,从指定的基点移动到目标点,并根据目标点 ;;
;;;旋转45度,然后再以目标点放大2倍.固然,这个程序完全可以用命 ;;
;;;令方式或者vla方式来完成。此处仅仅说明如何运用矩阵。        ;;
;;;注意:CAD的矩阵和OpenGL或其他的语言的矩阵有区别:           ;;
;;;      1.它们的矩阵是互为转置的。                           ;;
;;;      2.它们的矩阵相乘也是顺序相反的。                     ;;
;;;-----------------------------------------------------------;;
(defun c:test (/ ss p1 p2 mat1 mat2 mat3 i e o)
  (if (setq ss (ssget))                                         ;选择物体
    (progn
      (initget 1)
      (setq P1 (getpoint "n基点:"))                            ;指定基点
      (initget 1)
      (setq P2 (getpoint P1 "n目标点:"))                       ;指定目标点
      (grvecs (list 1 p1 p2))                                   ;红线标识位移
      (setq p1 (trans p1 1 0))                                  ;把输入得到的点转化为世界坐标系的点
      (setq p2 (trans p2 1 0))                                  ;把输入得到的点转化为世界坐标系的点
      (setq mat1 (MAT:TRANSLATEBY2P P1 p2))                     ;从P1位移到P2的位移矩阵
      (setq mat2 (MAT:ROTATION p2 (* pi 0.25)))                 ;以P2为基点旋转45度的变换矩阵
      (setq mat3 (MAT:SCALING p2 2.0))                          ;以P2为基点放大2倍变换矩阵
      (setq mat  (MAT:mxm mat3 (MAT:mxm mat2 mat1)))            ;须按照先后顺序从里到外这样相乘
      (setq mat  (vlax-tmatrix mat))                            ;用vlax-tmatrix把变换矩阵从表转化为ActiveX数组表达的矩阵
      (command "undo" "be")
      (setq i 0)
      (repeat (sslength ss)
        (setq e (ssname ss i))                                  ;获得图元名
        (setq o (vlax-ename->vla-object e))                     ;获得ActiveX对象
        (vla-transformby o mat)                                 ;用vla-transformby函数对之变换
        (setq i (1+ i))
      )
      (command "undo" "e")
    )
  )
  (princ)
)
;;;http://bbs.mjtd.com/forum.php?mod=viewthread&tid=91331
;;|;
(protect-assign MatLibSymbols)
(princ)


发表评论

电子邮件地址不会被公开。 必填项已用*标注