[编程交流] （3） 三维点到UCS拉伸

(setq p10 '(2 2 2)
     p11 '(4 2 2)
     p12 '(4 2 4))

;; Vector normal (Lee Mac)
(defun normal ( p1 p2 p3 )
 (one (vcv (mapcar '- p3 p2) (mapcar '- p3 p1)))
)
;; Vector x Scalar (Lee Mac)
(defun vxs ( v s )
 (mapcar '(lambda ( n ) (* n s)) v)
)
;; Vector norm (Lee Mac)
(defun nrm ( v )
 (sqrt (apply '+ (mapcar '(lambda ( n ) (* n n)) v)))
)
;; Unit Vector (Lee Mac)
(defun one ( v )
 ( (lambda ( n ) (if (equal 0.0 n 1e-14) nil (vxs v (/ 1.0 n)))) (nrm v))
)
;; Vector Cross Product (Lee Mac)
(defun vcv ( 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)))
 )
)

_$ (normal p10 p11 p12)
(0.0 1.0 0.0)

;; AnglePlanePlane
;; Returns the angle (radians) between two planes (using Al Kashi theorem)
;; the returned angle is always between 0 and pi radians
;;
;; Arguments: the single unit normal vectors of the planes
(defun AnglePlanePlane (nor1 nor2)
 ((lambda (d)
    (acos (/ (- 2 (* d d)) 2))
  )
   (distance nor1 nor2)
 )
)
;;; Acos
;;; Returns the Arc Cosine of number
;;;
;;; Argument: a number between -1 and 1
(defun acos (num)
 (cond
   ((equal num 1 1e-9) 0.0)
   ((equal num -1 1e-9) pi)
   ((< -1 num 1)
    (atan (sqrt (- 1 (expt num 2))) num)
   )
 )
)
;; CrossProduct
;; Returns the cross product (vector) of two vectors
;;
;; Arguments: two vectors
(defun CrossProduct (v1 v2)
 (list    (- (* (cadr v1) (caddr v2)) (* (caddr v1) (cadr v2)))
   (- (* (caddr v1) (car v2)) (* (car v1) (caddr v2)))
   (- (* (car v1) (cadr v2)) (* (cadr v1) (car v2)))
 )
)
;; Normalize
;; Returns the single unit vector of a vector
;;
;; Argument : un vecteur
(defun Normalize (v)
 ((lambda (l)
    (if (/= 0 l)
      (mapcar (function (lambda (x) (/ x l))) v)
    )
  )
   (distance '(0 0 0) v)
 )
)
;; Norm_3Points
;; Returns the single unit normal vector of a plane definedby 3 points
;;
;; Arguments: three points
(defun norm_3pts (p0 p1 p2)
 (Normalize (CrossProduct (mapcar '- p1 p0) (mapcar '- p2 p0)))
)

;; (return normal to plane defined by 3 points)
;**********************************************************
; Вектор нормали к плоскости. Плоскость задана 3-мя точкам,
;**********************************************************
; P1, P2, P3 - точки
; Возвращает вектор нормали. (P1 P2 P3 не на одной прямой)
;Примечание: в большинстве функций библиотеки полскость задается точкой и вектором нормали к ней
;Если в Вашем случае плоскость задана 3-мя точками используйте эту функцию для вычисления вектора нормали
(defun 3d_WNormp (P1 P2 P3 / W1 W2)
 (setq W1 (mapcar '- P2 P1)
       W2 (mapcar '- P3 P2)
 )
 (list (- (* (cadr W1)(caddr W2)) (* (caddr W1)(cadr W2)))
       (- (* (caddr W1)(car W2)) (* (car W1)(caddr W2)))
       (- (* (car W1)(cadr W2)) (* (cadr W1)(car W2))) 
 )
)

;;;Are Points CoLinear
;;;ARG -> 3 Points and Fuzz
;;;RET -> T nil
(defun is_pt_colinear (p1 p2 p3 fz)
 (or (equal (distance p1 p3)
            (+ (distance p1 p2)
               (distance p2 p3)) fz)
     (equal (distance p1 p2)
            (+ (distance p1 p3)
               (distance p3 p2)) fz)
     (equal (distance p2 p3)
            (+ (distance p1 p2)
               (distance p1 p3)) fz)))

;; Collinearity check (Lee Mac)
(defun isCollinear ( p1 p2 p3 fz )
 (equal (rem (angle p1 p2) pi) (rem (angle p2 p3) pi) fz)
)