;|
矩阵函数，fork自高飞鸟和lee mac
|;

;矢量函数




;;;name:BF-Vec-VxS
;;;desc:矢量乘标量
;;;arg:v:矢量
;;;arg:s:标量
;;;return:矢量
;;;example:(BF-Vec-VxS '(1 2 3) 3)
(defun BF-Vec-VxS (v s)
  (mapcar '(lambda (n) (* n s)) v)
)

;;;name:BF-Vec-Norm
;;;desc:矢量的模（长度）
;;;arg:v:矢量
;;;return:长度
;;;example:(BF-Vec-Norm '(1 2 3))
(defun BF-Vec-Norm (v)
  (sqrt (apply '+ (mapcar '* v v)))
)

;;;name:BF-Vec-Unit
;;;desc:单位矢量
;;;arg:v:矢量
;;;return:单位矢量
;;;example:(BF-Vec-Unit '(1 2 3))
(defun BF-Vec-Unit (v)
  ((lambda (n) 
		 (if (equal 0.0 n 1e-14)
			 nil 
			 (BF-Vec-VxS v (/ 1.0 n)))
	 ) 
		(BF-Vec-Norm v))
)

;;;name:BF-Vec-V+V
;;;desc:矢量相加
;;;arg:v1:矢量1
;;;arg:v2:矢量2
;;;return:矢量
;;;example:(BF-Vec-V+V '(1 2 3) '(3 2 1))
(defun BF-Vec-V+V (v1 v2)
  (mapcar '+ v1 v2)
)     

;;;name:BF-Vec-V-V
;;;desc:矢量相减
;;;arg:v1:矢量1
;;;arg:v2:矢量2
;;;return:矢量
;;;example:(BF-Vec-V-V '(1 2 3) '(3 2 1))
(defun BF-Vec-V-V (v1 v2)
  (mapcar '- v1 v2)
)

;;;name:BF-Vec-Dot
;;;desc:两矢量的点积 
;;;arg:v1:矢量1
;;;arg:v2:矢量2
;;;return:标量
;;;example:(BF-Vec-Dot '(1 2 3) '(3 2 1))
(defun BF-Vec-Dot (v1 v2)
  (apply '+ (mapcar '* v1 v2))
)

;;;name:BF-Vec-VxV
;;;desc:两矢量的叉积 
;;;arg:v1:矢量1
;;;arg:v2:矢量2
;;;return:矢量
;;;example:(BF-Vec-VxV '(1 2 3) '(3 2 1))
(defun BF-Vec-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)))
  )
)


;***********************************************************;;
; 矩阵部分                                                  ;;
;***********************************************************;;

;;;name:BF-Mat-Trp
;;;desc:矩阵转置  
;;;arg:m:矩阵
;;;return:转置后的矩阵
;;;example:(BF-Mat-Trp '((1 2 3) (3 2 1)))
(defun BF-Mat-Trp (m)
  (apply 'mapcar (cons 'list m))
)

;;;name:BF-Mat-M+M
;;;desc:矩阵相加
;;;arg:m:矩阵1
;;;arg:n:矩阵2
;;;return:矩阵
;;;example:(BF-Mat-M+M '((1 3 1) (1 0 0)) '((0 0 5) (7 5 0)))
(defun BF-Mat-M+M (m n)
  (mapcar '(lambda (r s) (mapcar '+ r s)) m n)
)

;;;name:BF-Mat-M-M
;;;desc:矩阵相减
;;;arg:m:矩阵1
;;;arg:n:矩阵2
;;;return:矩阵
;;;example:(BF-Mat-M-M '((1 3 1) (1 0 0)) '((0 0 5) (7 5 0)))
(defun BF-Mat-M-M (m n)
  (mapcar '(lambda (r s) (mapcar '- r s)) m n)
)

;;;name:BF-Mat-MxM
;;;desc:矩阵相乘
;;;arg:m:矩阵mxn
;;;arg:n:矩阵nxp
;;;return:矩阵mxp
;;;example:(BF-Mat-MxM '((1 0 2) (-1 3 1)) '((3 1) (2 1)(1 0)))
(defun BF-Mat-MxM (m q)
  (mapcar (function (lambda (r) (BF-Mat-MxV (BF-Mat-Trp q) r))) m)
)

;;;name:BF-Mat-MxV
;;;desc:矢量或点的矩阵变换(矢量乘矩阵) 
;;;arg:m:矩阵nxn
;;;arg:v:矢量R^n
;;;return:矢量R^n
;;;example:(BF-Mat-MxV '((1 0 2) (-1 3 1) (3 12 1)) '(2 11 0))
(defun BF-Mat-MxV (m v)
  (mapcar (function (lambda (r) (apply '+ (mapcar '* r v)))) m)
)


;;;name:BF-Mat-MxP
;;;desc:点的矩阵(4x4 matrix) 变换
;;;arg:m:矩阵4x4
;;;arg:p:三维点
;;;return:点变换后的位置
;;;example:(BF-Mat-MxP '((1 0 0 1) (0 1 0 1) (0 0 1 0) (0 0 0 1)) '(2 11 0))
(defun BF-Mat-MxP (m p)
  (reverse (cdr (reverse (BF-Mat-MxV m (append p '(1.0))))))
)


;;;name:BF-Mat-MxS
;;;desc:矩阵数乘
;;;arg:m:矩阵nxn
;;;arg:s:数
;;;return:矩阵
;;;example:(BF-Mat-MxS '((1 0 0 1) (0 1 0 1) (0 0 1 0) (0 0 0 1)) 2)
(defun BF-Mat-MxS (m s)
  (mapcar (function (lambda (v)(BF-Vec-VxS v s))) m)
)


;***********************************************************;;
;矩阵之变换部分                                             ;;
;***********************************************************;;

;;;name:BF-Mat-Translation
;;;desc:根据矢量计算平移矩阵
;;;arg:v:平移矢量
;;;return:4X4的平移矩阵
;;;example:(BF-Mat-Translation '(1 0 0))
(defun BF-Mat-Translation (v)
  (list
    (list 1. 0. 0. (float (car v)))
    (list 0. 1. 0. (float (cadr v)))
    (list 0. 0. 1. (float (caddr v)))
    (list 0. 0. 0. 1.)
  )
)

;;;name:BF-Mat-TranslateBy2P
;;;desc:根据两点计算平移矩阵
;;;arg:p1:基点
;;;arg:p2:目标点
;;;return:4X4的平移矩阵
;;;example:(BF-Mat-TranslateBy2P '(1 0 0) '(2 3 0))
(defun BF-Mat-TranslateBy2P (p1 p2)
  (BF-Mat-Translation (mapcar '- p2 p1))
)
;;;name:BF-Mat-TranslateBymat
;;;desc:根据矩阵和矢量计算平移矩阵
;;;arg:mat:4X4矩阵
;;;arg:p2:矢量
;;;return:4X4的平移矩阵
;;;example:(BF-Mat-TranslateBymat '((1 0 0 1) (0 1 0 1) (0 0 1 0) (0 0 0 1)) '(2 2 0))
(defun BF-Mat-TranslateBymat (mat p2)
  (BF-Mat-Translation (BF-Mat-MxV mat (append p2 '(1.0))))
)

;;;name:BF-Mat-Scaling
;;;desc:根据基点和缩放比例计算缩放矩阵
;;;arg:Cen:基点
;;;arg:scale:缩放比例
;;;return:4X4的缩放矩阵
;;;example:(BF-Mat-Scaling '(1 0 0) 2)
(defun BF-Mat-Scaling (Cen scale / s)
  (setq s (- 1 (setq scale (float 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.)
  )
)

;;;name:BF-Mat-Rotation
;;;desc:根据基点和旋转角度计算旋转矩阵
;;;arg:Cen:基点
;;;arg:ang:旋转角度
;;;return:4X4的旋转矩阵
;;;example:(BF-Mat-Rotation '(1 0 0) (/ pi 3))
(defun BF-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.)
  )
)


;;;name:BF-Mat-Rotation3D
;;;desc:根据基点、旋转轴矢量和旋转角度计算旋转矩阵
;;;arg:Cen:基点
;;;arg:Axis:旋转轴矢量
;;;arg:ang:旋转角度
;;;return:4X4的旋转矩阵
;;;example:(BF-Mat-Rotation3D '(1 0 0) '(1 0 0) (/ pi 3))
(defun BF-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 (BF-Mat-MxV M Cen))
			M (BF-Mat-DispToMatrix M p)
    )
  )
)


;;;name:BF-Mat-RotateBy2P
;;;desc:根据旋转轴矢量两点和旋转角度计算旋转矩阵
;;;arg:p1:旋转轴矢量第一点
;;;arg:p2:旋转轴矢量第二点
;;;arg:ang:旋转角度
;;;return:4X4的旋转矩阵
;;;example:(BF-Mat-RotateBy2P '(1 0 0) '(2 2 0) (/ pi 3))
(defun BF-Mat-RotateBy2P (p1 p2 ang)
	(BF-Mat-Rotation3D p1 (mapcar '- p2 p1) ang)
)


;;;name:BF-Mat-Reflect
;;;desc:根据反射（镜像）轴矢量两点计算反射（镜像）矩阵
;;;arg:p1:反射（镜像）矢量第一点
;;;arg:p2:反射（镜像）矢量第二点
;;;return:4X4的反射（镜像）矩阵
;;;example:(BF-Mat-Reflect (getpoint) (getpoint))
(defun BF-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.)
  )
)


;;;name:BF-Mat-mirroring
;;;desc:根据反射（镜像）平面计算反射（镜像）矩阵
;;;arg:p1:反射（镜像）平面第一点
;;;arg:p2:反射（镜像）平面第二点
;;;arg:p3:反射（镜像）平面第三点
;;;return:4X4的反射（镜像）矩阵
;;;example:(BF-Mat-mirroring (getpoint) (getpoint) (getpoint))
(defun BF-Mat-mirroring (p1 p2 p3 / m ux uy uz)
  (mapcar
    'set
    '(ux uy uz)
    (BF-Vec-Unit (BF-Vec-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)))
					)
  )
  (BF-Mat-DispToMatrix m (mapcar '- p1 (BF-Mat-mxv m p1)))
)

;;;name:BF-Mat-DispToMatrix
;;;desc:把位移矢量添加到矩阵中
;;;arg:mat:3X3矩阵
;;;arg:disp:位移矢量
;;;return:4X4的平移矩阵
;;;example:(BF-Mat-DispToMatrix '((1 0 2) (-1 3 1) (3 12 1)) '(2 11 0))
(defun BF-Mat-DispToMatrix (mat disp)
  (append
    (mapcar 'append mat (mapcar 'list disp))
    '((0. 0. 0. 1.))
  )
)




;***********************************************************;;
; 以下部分来自Lee-Mac,特地致谢!                             ;;
;***********************************************************;;
;;;name:BF-Mat-ScaleByMatrix
;;;desc:点或对象的矩阵缩放变换
;;;arg:target:点表或VLA对象
;;;arg:p1:缩放基点
;;;arg:scale:缩放比例
;;;return:缩放后的对象或点表
;;;example:(BF-Mat-ScaleByMatrix '((1 0 0) (0 1 0) (0 0 1) (0 0 0)) (getpoint) 2)
(defun BF-Mat-ScaleByMatrix (target p1 scale / m)
  (BF-Mat-ApplyMatrixTransformation target
    (setq m
      (list
        (list scale 0. 0.)
        (list 0. scale 0.)
        (list 0. 0. scale)
      )
    )
    (mapcar '- p1 (BF-Mat-MxV m p1))
  )
)

;;;name:BF-Mat-TranslateByMatrix
;;;desc:点或对象的矩阵平移变换
;;;arg:target:点表或VLA对象
;;;arg:p1:基点
;;;arg:p2:目标点
;;;return:平移后的对象或点表
;;;example:(BF-Mat-TranslateByMatrix '((1 0 0) (0 1 0) (0 0 1) (0 0 0)) (getpoint) (getpoint))
(defun BF-Mat-TranslateByMatrix (target p1 p2)
  (BF-Mat-ApplyMatrixTransformation target
    (list
      (list 1. 0. 0.)
      (list 0. 1. 0.)
      (list 0. 0. 1.)
    )
    (mapcar '- p2 p1)
  )
)


;;;name:BF-Mat-RotateByMatrix
;;;desc:点或对象的矩阵旋转变换
;;;arg:target:点表或VLA对象
;;;arg:p1:基点
;;;arg:ang:旋转角度
;;;return:旋转后的对象或点表
;;;example:(BF-Mat-RotateByMatrix '((1 0 0) (0 1 0) (0 0 1) (0 0 0)) (getpoint) (/ pi 6))
(defun BF-Mat-RotateByMatrix (target p1 ang / m)
  (BF-Mat-ApplyMatrixTransformation target
    (setq m
      (list
        (list (cos ang) (- (sin ang)) 0.)
        (list (sin ang) (cos ang) 0.)
        (list 0. 0. 1.)
      )
    )
    (mapcar '- p1 (BF-Mat-MxV m p1))
  )
)


;;;name:BF-Mat-ReflectByMatrix
;;;desc:点或对象的矩阵反射（镜像）变换
;;;arg:target:点表或VLA对象
;;;arg:p1:反射（镜像）矢量第一点
;;;arg:p2:反射（镜像）矢量第二点
;;;return:反射（镜像）后的对象或点表
;;;example:(BF-Mat-ReflectByMatrix '((1 0 0) (0 1 0) (0 0 1) (0 0 0)) (getpoint) (getpoint))
(defun BF-Mat-ReflectByMatrix (target p1 p2 / m)
  (
    (lambda ( a / m )
      (BF-Mat-ApplyMatrixTransformation target
        (setq m
          (list
            (list (cos a)    (sin a)  0.)
            (list (sin a) (- (cos a)) 0.)
            (list    0.         0.    1.)
          )
        )
        (mapcar '- p1 (BF-Mat-MxV m p1))
      )
    )
    (* 2. (angle p1 p2))
  )
)


;;;name:BF-Mat-ApplyMatrixTransformation
;;;desc:点或对象的矩阵变换
;;;arg:target:点表或VLA对象
;;;arg:matrix:3x3 矩阵
;;;arg:vector:移动矢量
;;;return:矩阵变换后的对象或点表
;;;example:
(defun BF-Mat-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 '+ (BF-Mat-MxV matrix point) vector))
        )
        target
      )
    )        
  )
)

;---------------=={      三维变换       }==-----------------;;
;-----------------------------------------------------------;;
;;;name:BF-Mat-Rotate3D
;;;desc:点或对象的矩阵3维旋转变换
;;;arg:target:点表或VLA对象
;;;arg:p1:旋转轴矢量的第一点
;;;arg:p2:旋转轴矢量的第二点
;;;arg:ang:旋转角度
;;;return:旋转后的对象或点表
;;;example:(BF-Mat-Rotate3D '((1 0 0) (0 1 0) (0 0 1) (0 0 0)) (getpoint) (getpoint)(/ pi 6))
(defun BF-Mat-Rotate3D (target p1 p2 ang / ux uy uz u m)
  (mapcar 'set '(ux uy uz) (setq u (BF-Vec-Unit (mapcar '- p2 p1))))
  (BF-Mat-ApplyMatrixTransformation target
    (setq m
      (BF-Mat-M+M
        (list
          (list (cos ang) 0. 0.)
          (list 0. (cos ang) 0.)
          (list 0. 0. (cos ang))
        )
        (BF-Mat-M+M
          (BF-Mat-MxS
            (list
              (list 0. (- uz) uy)
              (list uz 0. (- ux))
              (list (- uy) ux 0.)
            )
            (sin ang)
          )
          (BF-Mat-MxS (mapcar (function (lambda (e) (BF-Vec-VxS u e))) u) (- 1. (cos ang)))
        )
      )
    )      
    (mapcar '- p1 (BF-Mat-MxV m p1))
  )
)

;;;name:BF-Mat-Reflect3D
;;;desc:点或对象的矩阵3维反射（镜像）变换
;;;arg:target:点表或VLA对象
;;;arg:p1:反射（镜像）平面第一点
;;;arg:p2:反射（镜像）平面第二点
;;;arg:p3:反射（镜像）平面第三点
;;;return:反射（镜像）后的对象或点表
;;;example:(BF-Mat-Reflect3D '((1 0 0) (0 1 0) (0 0 1) (0 0 0)) (getpoint) (getpoint)(getpoint))
(defun BF-Mat-Reflect3D (target p1 p2 p3 / m u ux uy uz )
  (mapcar 'set '(ux uy uz) (setq u (BF-Vec-Unit (BF-Vec-VxV (mapcar '- p2 p1) (mapcar '- p3 p1)))))
  (BF-Mat-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 (BF-Mat-MxV m p1))
  )
)