[编程交流] 从f建立一条平均线

;; fl3d, Orthogonal Regression in 3D   by ymg                                  ;
;;                                                                             ;
;; For Derivation of Formulas See Following Paper:                             ;
;;        3-D LINEAR REGRESSION  by Jean Jacquelin                             ;
;; http://www.scribd.com/doc/31477970/Regressions-et-trajectoires-3D           ;
;;                                                                             ;
(defun c:fl3d (/ *acaddoc* a avg b c0 c1 c2 cos2a cosa errl go k00 k01 k10 k11 k12 k22
               n p phi pl q r rho s2m sin2a sina ss sum sxx sxy sxz syy syz szz u v w xm ym zm)
  (vl-load-com)
  (defun *error* (msg)
             (mapcar 'eval errl)
(if (and msg (not (wcmatch (strcase msg) "*BREAK*,*CANCEL*,*EXIT*")))
          (princ (strcat "\nError: " msg))
       )
(and *AcadDoc* (vla-endundomark *AcadDoc*))
       (princ)
  )
 
  (defun acos (z)  (atan (sqrt (- 1.0 (* z z))) z))
 
  (setq errl '("CMDECHO" "DIMZIN")
        errl (mapcar (function (lambda (a) (list 'setvar a (getvar a)))) errl)
  )
    
    
  (or *AcadDoc*
     (setq *AcadDoc* (vla-get-activedocument (vlax-get-acad-object)))
  )
  
  (princ (strcat "\n Select Points"))
  (if (setq  n 0
     pl nil
     ss (ssget '((0 . "POINT")))
            go (> (sslength ss) 2) 
      )
     (progn
        (vla-startundomark *AcadDoc*)           
        (setvar 'CMDECHO 0)
        (setvar 'DIMZIN 0)
 (repeat (sslength ss)
         (setq pl (cons (cdr (assoc 10 (entget (ssname ss n)))) pl)
                n (1+ n)
         )
 )
        (setq  avg  (mapcar '/  (apply 'mapcar (cons '+ pl)) (list n n n))
               ;dif  (mapcar '(lambda (a) (mapcar '- a avg)) pl)
               sum  (apply
                       'mapcar
                          (cons '+
                             (mapcar
                                '(lambda (a)
                                     (list (* (car a) (car a))       ;  XX 
                                           (* (car a) (cadr a))      ;  XY 
                                           (* (cadr a) (cadr a))     ;  YY 
                                           (* (car a) (caddr a))     ;  XZ 
                                           (* (caddr a) (caddr a))   ;  ZZ 
                                           (* (cadr a) (caddr a))    ;  YZ 
                                     )
                                 )
                                 pl
                             )
                          )
                    )
               sum  (mapcar '/ sum (list n n n n n n))
               ;; avg contains:  (Xm Ym Zm)                                  ;
               ;; sum contains:  (sXX sXY sYY sXZ sZZ sYZ) Divided by n      ;
                  Xm (car avg) Ym (cadr avg) Zm (caddr avg)
                 Sxx (- (car sum)(* Xm Xm))
                 Sxy (- (cadr sum)(* Xm Ym))
                 Syy (- (caddr sum)(* Ym Ym))
                 Sxz (- (cadddr sum)(* Xm Zm))
                 Szz (- (car (cddddr sum))(* Zm Zm))
                 Syz (- (cadr (cddddr sum))(* Ym Zm))
                   a (/ (atan (/ (* Sxy 2.)(- Sxx Syy))) 2.)         
                Cosa (cos a)        Sina (sin a)
               Cos2a (* Cosa Cosa) Sin2a (* Sina Sina)            
                 K11 (+ (* (+ Syy Szz) Cos2a) (* (+ Sxx Szz) Sin2a) (* -2. Sxy Cosa Sina))
                 K22 (+ (* (+ Syy Szz) Sin2a) (* (+ Sxx Szz) Cos2a) (*  2. Sxy Cosa Sina))        
                 K12 (+ (* (* Sxy -1.)(- Cos2a Sin2a))(* (- Sxx Syy) Cosa Sina))
                 K10 (+ (* Sxz Cosa)(* Syz Sina))
                 K01 (+ (* Sxz Sina -1.)(* Syz Cosa))
                 K00 (+ Sxx Syy)           
                  c2 (* (+ K00 K11 K22) -1.)
                  c1 (+ (* K00 K11)(* K00 K22)(* K11 K22)(* K01 K01 -1.)(* K10 K10 -1.))
                  c0 (+ (* K01 K01 K11)(* K10 K10 K22)(* -1. K00 K11 K22))
                   p (- c1 (/ (* c2 c2) 3.))
                   q (+ (/ (* 2.0 c2 c2 c2) 27.)(/ (* -1. c1 c2) 3.) c0)
                   R (+ (/ (* q q) 4.)(/ (* p p p) 27.))         
        )
        (if (minusp R)
           (progn
               (setq rho (sqrt (/ (* -1. p p p) 27.))
                     phi (acos (/ q (* -2. rho)))
                     s2m (min (+ (/ c2 -3.)(* 2. (expt rho (/ 1. 3.))(cos (/ phi 3.))))
                              (+ (/ c2 -3.)(* 2. (expt rho (/ 1. 3.))(cos (/ (+ phi pi pi) 3.))))
                              (+ (/ c2 -3.)(* 2. (expt rho (/ 1. 3.))(cos (/ (+ phi pi pi pi pi) 3.))))
                         )
               )      
           )
           (setq s2m (+ (/ c2 -3.)(expt (+ (/ q -2.)(sqrt R)) (/ 1. 3.))(expt (- (/ q -2.)(sqrt R)) (/ 1. 3.))))
        )
        (setq a (+ (* (/ (* -1. K10)(- K11 s2m)) Cosa) (* (/ K01 (- K22 s2m)) Sina))
              b (+ (* (/ (* -1. K10)(- K11 s2m)) Sina) (* (/ (* -1. K01) (- K22 s2m)) Cosa))
              u (* (/ 1. (+ 1. (* a a) (* b b))) (+ (* (+ 1. (* b b)) Xm) (* -1. a b Ym) (* a Zm)))
              v (* (/ 1. (+ 1. (* a a) (* b b))) (+ (* -1 a b Xm) (* (+ 1 (* a a)) Ym) (* b Zm)))
              w (* (/ 1. (+ 1. (* a a) (* b b))) (+ (* a Xm) (* b Ym) (* (+ (* a a) (* b b)) Zm)))
        )      
        (entmakex (list '(0 . "LINE") (cons 10 avg) (cons 11 (list u v w))))
        
        
     ) 
  )
  (*error* nil)
)
(princ "\nFit a 3D Line to Selection Set of Points, Type FL3D to run")

5