代码拉取完成，页面将自动刷新
(defun C:FC (/ GET_UNIT GETPOINT_W
GETPOSREAL MAIN_FUN
GET_MIDPOINT PNTSS_TO_LST
TEST_FITMETHOD CIRCLE_FIT
CIRCLE_FIT_ONE_KPNT CIRCLE_FIT_TWO_END
TAUBIN_CIRCLE_FIT TAUBIN_CIRCLE_FIT_ONE_END
TAUBIN_CIRCLE_FIT_TWO_END
LINE_FIT LINE_DEMING_FIT
DRAW_RES
)
(defun get_unit(mark / unit units_lst)
(setq units_lst(list '("M" "m" 1.0 3)
'("D" "dm" 10.0 2)
'("C" "cm" 100.0 1)
'("N" "mm" 1000.0 1)))
(setq unit(car (vl-remove-if-not '(lambda(x) (equal (car x) mark)) units_lst)))
(if (not unit)
(setq unit(cadddr units_lst)))
unit
)
(defun getpoint_w ( name OSMODE / base_pnt endow)
(setq endow nil)
(while (not endow)
(setvar "OSMODE" OSMODE)
(setq base_pnt (getpoint name ))
(setvar "OSMODE" 0)
(if base_pnt
(setq endow T)
(alert "未选择到点位，请重新选择！")
)
)
base_pnt
)
(defun getposreal (str value / endow)
(setq endow T)
(while endow
(setq real (getreal str))
(if (and real (>= real value))
(setq endow nil)
(alert
(strcat
"输入不合法，请重新输入。可能原因为：\n1.要求输入数值为大于"
(rtos value 2 0)
",当前输入数值小于"
(rtos value 2 0)
";\n2.输入数值为空."
)
)
)
real
)
)
(defun main_fun (/ f_type res_w h thr obj
ss endow pnt_lst l_param R_pnt
f_type obj R unit_mark
unit sub_type
)
(initget "A C L")
(setq f_type (getkword "\n选择拟合线类型：[圆弧(A)/圆(C)/直线(L)]"))
(if (not (equal f_type "L"))
(progn
(initget "N O P")
(setq sub_type (getkword "\n选择圆弧约束类型：[无约束(N)/一点约束(O)/两点约束(P)]"))
(if (or (equal sub_type "O") (equal sub_type "P"))
(progn
(setq stpnt( getpoint_w "\n选择圆弧约束第一点：" 1))
(if (equal sub_type "P")
(setq endpnt( getpoint_w "\n选择圆弧约束第二点：" 1)))
)
)
)
)
(initget " Y N")
(setq res_w (getkword "\是否写入残差值：[是(Y)/否(N)]"))
(if (not (equal res_w "N"))
(progn
(setq t_mark T
h 0.2
thr 0.05
unit_mark "N"
)
(while t_mark
(initget " H T U")
(setq t_mark
(getkword"\输入残差值文本参数：[默认( 空格)/字高(H)/阈值(T)/单位(U)]")
)
(cond ((equal t_mark " ") (setq t_mark nil))
((equal t_mark "H")
(setq h (getposreal "\输入残差值字体高度(默认1.0)：" 0.00001)
t_mark T)
)
((equal t_mark "T")
(setq thr (getposreal "\输入残差值变色阈值(默认0.05m)：" 0.0)
t_mark T)
)
((equal t_mark "U")
(progn
(initget "M D C N")
(setq unit_mark
(getkword"\输入残差值单位：[米(M)/分米(D)/厘米(C)/毫米(N)]")
t_mark T
)
)
)
)
)
)
)
(setq unit(get_unit unit_mark))
(setq ss (ssget '((0 . "POINT"))))
(setq endow nil)
(while endow
(setq ss (ssget '((0 . "POINT"))))
(if (< (sslength ss) 4)
(progn
(setq endow T)
(alert (strcat "选择的点对象少于3个，无法拟合请重新选择！"))
)
(setq endow nil)
)
)
(setq pnt_lst (pntss_to_lst ss T))
(if (equal f_type "L")
(progn
(setq l_param (line_deming_fit pnt_lst)
R_pnt (mapcar '*
(mapcar '+ (cadr l_param) (caddr l_param))
(list 0.5 0.5)
)
)
(entmakex (list '(0 . "LINE")
'(8 . "fit_line")
(cons 10 (cadr l_param))
(cons 11 (caddr l_param))
)
)
)
(progn
(cond ((equal sub_type "O") (setq c_param (taubin_circle_fit_one_end pnt_lst st_pnt 20)))
((equal sub_type "P") (setq c_param (taubin_circle_fit_two_end pnt_lst stpnt endpnt)))
(T (setq c_param (taubin_circle_fit pnt_lst 20)))
)
(if (equal f_type "A")
(entmakex (list '(0 . "ARC")
'(8 . "fit_line")
(cons 10 (car c_param))
(cons 40 (cadr c_param))
(cons 50 (caddr c_param))
(cons 51 (cadddr c_param))
)
)
(entmakex (list '(0 . "CIRCLE")
'(8 . "fit_line")
(cons 10 (car c_param))
(cons 40 (cadr c_param))
)
)
)
)
)
(if (not (equal res_w "N"))
(progn
(setq obj (entlast)
R (draw_res obj pnt_lst h 0.0 thr unit)
R (strcat "拟合残差：" (rtos R 2 (last unit)) (cadr unit))
)
(entmakex (list '(0 . "TEXT")
'(8 . "Residuals")
(cons 10 (get_midpoint obj))
(cons 40 (* 1.5 h))
(cons 50 0.0)
(cons 62 2)
(cons 1 R)
)
)
)
)
)
(defun get_midpoint (obj)
;;圆弧多线段 线段中点
(setq obj (vlax-ename->vla-object obj))
(vlax-curve-GetPointAtDist
obj
(/ (vlax-curve-GetDistAtParam obj (vlax-curve-GetEndParam obj))
2.
)
)
)
(defun pntss_to_lst (ss elev / i pnt_lst)
;点集合转化成坐标点表，el 表示是否需要将点高程归零
(setq pnt_lst nil
i 0
)
(repeat (sslength ss)
(setq pnt (cdr (assoc 10 (entget (ssname ss i)))))
(if (not elev)
(setq pnt_lst (cons pnt pnt_lst))
(setq pnt (list (car pnt) (cadr pnt) 0.0)
pnt_lst (cons pnt pnt_lst)
)
)
(setq i (1+ i))
)
pnt_lst
)
;;************************************************************************
;;** 测试 圆弧拟合方法 **
;;************************************************************************
(defun test_fitmethod (/ stpnt endpnt pnt_lst)
(defun entmake_arc (fff /)
(entmakex (list '(0 . "ARC")
'(8 . "fit_line")
(cons 10 (car fff))
(cons 40 (cadr fff))
(cons 50 (caddr fff))
(cons 51 (cadddr fff))
)
)
)
;;测试圆弧拟合方法正确性
(setq stpnt (getpoint)
endpnt (getpoint)
)
(setq pnt_lst (pntss_to_lst (ssget '((0 . "POINT"))) T))
(setq fff (circle_fit_one_kpnt stpnt pnt_lst))
(setq ddd (LM_method_end pnt_lst stpnt 50))
(setq ffsdds (taubin_circle_fit_two_end pnt_lst stpnt endpnt))
(entmake_arc ddd)
)
;;************************************************************************
;;** start LM几何拟合 **
;;************************************************************************
(defun LM_method_end(pnt_lst stpnt max_iter / lam
mark i n avg
epsilon stpnt_av pnt_av new_par
old_par jg_m lam_mark dx_dy
dR new_cen new_r
)
(defun cal_sigma(pntlst circle_par / )
(apply '+ (mapcar '(lambda(x) (* (- (cadr circle_par) (circle_sqrt x (car circle_par)))
(- (cadr circle_par) (circle_sqrt x (car circle_par))))) pntlst))
)
(defun circle_sqrt(xy ab )
(sqrt (apply '+ (mapcar '* (mapcar '- xy ab)(mapcar '- xy ab)))))
(defun xyztojb(ipnt stpnt param / J_xc J_yc g_xc g_yc st_sqrt i_sqrt)
(setq st_sqrt (circle_sqrt stpnt (car param))
i_sqrt (circle_sqrt ipnt (car param))
J_xc (+ (/ (- (car (car param)) (car stpnt)) st_sqrt)
(/ (- (car ipnt) (car (car param))) i_sqrt)
)
J_yc (+ (/ (- (cadr (car param)) (cadr stpnt)) st_sqrt)
(/ (- (cadr ipnt) (cadr (car param))) i_sqrt)
)
g_xc (* J_xc (- st_sqrt i_sqrt))
g_yc (* J_yc (- st_sqrt i_sqrt))
)
(list (* J_xc J_xc)
(* J_xc J_yc)
(* J_yc J_yc)
g_xc g_yc)
)
(defun cal_dxdy(J11 J12 J22 G1 G2 lam / det dx dy)
(setq det(- (* J11 J22 (1+ lam) (1+ lam))
(* J12 J12))
dy(/ (-(* G2 J11 (1+ lam))
(* G1 J12))
det)
dx(/ (-(* G1 J22 (1+ lam))
(* G2 J12))
det))
(list dx dy)
)
(setq mark T i 0
lam 0.001
epsilon 3.e-8
n (length pnt_lst)
avg (mapcar '/
(apply 'mapcar (cons '+ pnt_lst))
(list (float n) (float n))
)
stpnt_av(mapcar '- stpnt avg)
pnt_av(mapcar '(lambda (a) (mapcar '- a avg)) pnt_lst)
;new_par(circle_fit pnt_lst)
;new_par(list (car new_par) (distance (car new_par) stpnt_av))
new_par(list '(0 0) (distance '(0 0) stpnt_av))
)
(while (and mark (< i max_iter))
(setq old_par new_par
jg_m (apply
'mapcar
(cons '+
(mapcar
'(lambda (a)
(xyztojb a stpnt_av old_par
)
)
pnt_av
)
)
);;计算雅可比矩阵
jg_m(mapcar '(lambda(x) (/ x n)) jg_m)
lam_mark T)
(while lam_mark
(setq dx_dy(cal_dxdy (car jg_m) (cadr jg_m) (caddr jg_m) (cadddr jg_m) (last jg_m) lam)
dR(sqrt (apply '+ (mapcar '* dx_dy dx_dy))))
(if (< (/ (apply '+ (mapcar 'abs (cons dR dx_dy))) (1+ (cadr old_par))) epsilon)
(setq lam_mark nil mark nil)
(progn
(setq new_cen(mapcar '- (car old_par) dx_dy)
new_r(distance new_cen stpnt_av)
new_par(list new_cen new_r)
new_sigma(cal_sigma pnt_av new_par)
)
(if (apply 'or (mapcar '> (mapcar 'abs new_cen) (list 1.e+6 1.e+6)))
(setq lam_mark nil mark nil)
(if (< new_sigma (cal_sigma pnt_av old_par))
(setq lam(* lam 0.01) ;;减小步长靠近极值点
lam_mark nil mark T)
(setq lam(* lam 10) ;;增加步长跳出单调增加区域
lam_mark T mark T))
)
)
)
(setq i(1+ i))
)
(setq i(1+ i))
)
(setq cen (mapcar '+ (car new_par) avg)
radian (vl-sort
(mapcar '(lambda (x) (angle cen x)) (cons stpnt pnt_lst))
'(lambda (e1 e2) (< e1 e2))
)
)
(list cen (cadr new_par) (car radian) (last radian))
)
;;************************************************************************
;;** end LM几何拟合 **
;;************************************************************************
;;************************************************************************************
;;** 最小化目标为以下函数的圆弧拟合方法（无约束，一个端点约束，两个端点约束) **
;;** **
;;** f(xc, yc, r) = ((x - xc)^2 + (y - yc)^2 - r^2)^2 **
;;** **
;;** 这种方法为一种经典代数拟合方法，优点是计算速度快，但是拟合圆弧可能会偏小*****
;;************************************************************************************
;;************************************************************************
;;** start 无约束的圆弧拟合 **
;;************************************************************************
(defun circle_fit (pnt_lst / n avg dif sum a b m s cv cu cen rad radian)
;;(setq pnt_lst(list '(12 23) '(23 32))) kasa fit
(setq n (length pnt_lst)
avg (mapcar '/
(apply 'mapcar (cons '+ pnt_lst))
(list (float n) (float n) (float n))
)
dif (mapcar '(lambda (a) (mapcar '- a avg)) pnt_lst)
sum (apply
'mapcar
(cons '+
(mapcar
'(lambda (a)
(list (* (car a) (car a))
(* (car a) (cadr a))
(* (cadr a) (cadr a))
(* (car a) (car a) (car a))
(* (car a) (cadr a) (cadr a))
(* (car a) (car a) (cadr a))
(* (cadr a) (cadr a) (cadr a))
)
)
dif
)
)
)
;; sum contains: (sUU sUV sVV sUUU sUVV sUUV sVVV) ;
a (list (car sum)
(cadr sum)
(/ (+ (cadddr sum) (car (cddddr sum))) 2.)
)
b (list (cadr sum)
(caddr sum)
(/ (+ (cadr (cddddr sum)) (caddr (cddddr sum))) 2.)
)
m (/ (car a) (car b))
s (mapcar '- a (mapcar '(lambda (x) (* x m)) b))
cv (/ (caddr s) (cadr s))
cu (/ (- (caddr a) (* (cadr a) cv)) (car a))
cen (mapcar '+ (list cu cv) avg)
rad (sqrt
(+ (* cu cu) (* cv cv) (/ (+ (car sum) (caddr sum)) n))
)
)
(setq radian (vl-sort (mapcar '(lambda (x) (angle cen x)) pnt_lst)
'(lambda (e1 e2) (< e1 e2))
)
)
(list cen rad (car radian) (last radian))
)
;;************************************************************************
;;** end 无约束的圆弧拟合 **
;;************************************************************************
;;************************************************************************
;;** start 一个端点约束的圆弧拟合 **
;;************************************************************************
(defun circle_fit_one_kpnt (pnt_lst kpnt / to_xy n n_list dif sum a b m s cv cu cen rad radian)
(defun to_xy (xy pq / xp yq xy2pq2)
(setq xp (- (car pq) (car xy))
yq (- (cadr pq) (cadr xy))
xy2pq2 (- (apply '+ (mapcar '* xy xy))
(apply '+ (mapcar '* pq pq))
)
)
(list (* xp xp)
;;Suu
(* xp yq)
;; Suv
(* yq yq)
;;Svv
(* -1.0 xp xy2pq2)
;;Suuu
(* -1.0 yq xy2pq2)
;;Suvv
)
)
(setq n (length pnt_lst)
n_list (list (float n) (float n) (float n))
avg (mapcar '/
(apply 'mapcar (cons '+ pnt_lst))
n_list
)
sum (apply
'mapcar
(cons '+
(mapcar
'(lambda (a)
(to_xy (mapcar '- a avg) (mapcar '- kpnt avg))
)
pnt_lst
)
)
)
a (list (car sum)
(cadr sum)
(/ (cadddr sum) 2.)
)
b (list (cadr sum)
(caddr sum)
(/ (last sum) 2.)
)
m (/ (car a) (car b))
s (mapcar '- a (mapcar '(lambda (x) (* x m)) b))
cv (/ (caddr s) (cadr s))
cu (/ (- (caddr a) (* (cadr a) cv)) (car a))
cen (mapcar '+ (list cu cv) avg)
rad (distance cen (list (car kpnt) (cadr kpnt)))
)
(setq radian (vl-sort (mapcar '(lambda (x) (angle cen x)) (cons kpnt pnt_lst))
'(lambda (e1 e2) (< e1 e2))
)
)
(list cen rad (car radian) (last radian))
)
;;************************************************************************
;;** end 一个端点约束的圆弧拟合 **
;;************************************************************************
;;************************************************************************
;;** start 二个端点约束的圆弧拟合 **
;;************************************************************************
(defun circle_fit_two_end (pnt_lst stpnt endpnt / n
n_list dif sum a b m
s cv cu cen rad radian
fmx perpendicular
)
(defun fmxy (pnt st end / pnt-st pnt2-st2 st-end st2-end2 fm)
(setq pnt-st (mapcar '- pnt st)
pnt2-st2 (apply '+
(mapcar '* (mapcar '- pnt st) (mapcar '+ pnt st))
)
st-end (mapcar '- st end)
st2-end2 (apply '+
(mapcar '* (mapcar '- st end) (mapcar '+ st end))
)
fm (apply '- (mapcar '* pnt-st (reverse st-end)))
)
(list (* fm
(apply '-
(mapcar '*
(list (cadr st-end) (cadr pnt-st))
(list pnt2-st2 st2-end2)
)
)
)
(* -1.0 fm
(apply '-
(mapcar '*
(list (car st-end) (car pnt-st))
(list pnt2-st2 st2-end2)
)
)
)
(* 2.0 fm fm)
)
)
(defun perpendicular (st end / st-end)
;;Ay = Bx + C
(setq st-end (mapcar '- st end))
(list (cadr st-end)
(* -1.0 (car st-end))
(* 0.5 (apply '+ (mapcar '* st-end (mapcar '+ st end))))
)
)
(setq n (length pnt_lst)
avg (mapcar '/
(apply 'mapcar (cons '+ pnt_lst))
(list (float n) (float n))
)
sum (apply
'mapcar
(cons '+
(mapcar
'(lambda (a)
(fmxy (mapcar '- a avg)
(mapcar '- stpnt avg)
(mapcar '- endpnt avg)
)
)
pnt_lst
)
)
)
cu (/ (car sum) (caddr sum))
cv (/ (cadr sum) (caddr sum))
l_p (perpendicular
(mapcar '- stpnt avg)
(mapcar '- endpnt avg)
)
)
;;; (if (< (abs(car l_p)) 0.0000000001)
;;; (setq cv (cadr stpnt))
;;; (setq
;;; cv (/ (apply '+ (mapcar '* (cdr l_p) (list cu 1)))
;;; (car l_p)
;;; )
;;; )
;;; )
(setq cen (mapcar '+ (list cu cv) avg)
rad (distance cen (list (car stpnt) (cadr stpnt)))
radian (vl-sort (mapcar '(lambda (x) (angle cen x)) (append (list stpnt endpnt) pnt_lst))
'(lambda (e1 e2) (< e1 e2))
)
)
(list cen rad (car radian) (last radian))
)
;;************************************************************************
;;** end 二个端点约束的圆弧拟合 **
;;************************************************************************
;;************************************************************************************
;;** 最小化目标为以下函数的圆弧拟合方法（无约束，一个端点约束，两个端点约束) **
;;** **
;;** f(xc, yc, r) = ((x - xc)^2 + (y - yc)^2 - r^2)^2 **
;;** **
;;** 结束 **
;;************************************************************************************
;;*—————————————————————————————————————————————————————————————
;;************************************************************************************
;;** 最小化目标为以下函数的圆弧拟合方法（无约束，一个端点约束，两个端点约束) **
;;** **
;;** ((x - xc)^2 + (y - yc)^2 - r^2)^2 **
;;** f(xc, yc, r) = --------------------------------- **
;;** ((x - xc)^2 + (y - yc)^2) **
;;** **
;;** 这种方法为taubin方法，相当于最小化距离的一阶泰勒展开式子 **
;;** 这种方法计算速度也很快无需进行矩阵运算，特征方程只需要4-6次迭代就可以求得 **
;;************************************************************************************
;;************************************************************************
;;** start 无约束的圆弧拟合 **
;;************************************************************************
(defun taubin_circle_fit (pnt_lst max_iter / n
xy_to_xyz avg xyz M_s
M_s M_xy M_xx M_yy M_xz
M_yz M_zz M_z cov_xy var_z
A3 A2 A1 A0 A22
A33 mark i iter_x iter_y
delta_y x_new y_new DET x_c
y_c rad radian
)
(defun xy_to_xyz (xy / z)
(setq z (apply '+ (mapcar '* xy xy)))
(list (car xy) (cadr xy) z)
)
(defun square_root (a b c / sqrt_b2_4ac)
(setq sqrt_b2_4ac (sqrt (- (* b b) (* 4.0 a c))))
(list (/ (+ b sqrt_b2_4ac) -2.0 a)
(/ (- b sqrt_b2_4ac) -2.0 a)
)
)
(setq n (length pnt_lst)
avg (mapcar '/
(apply 'mapcar (cons '+ pnt_lst))
(list (float n) (float n) (float n))
)
xyz (mapcar '(lambda (a) (xy_to_xyz (mapcar '- a avg))) pnt_lst)
M_s (apply
'mapcar
(cons '+
(mapcar
'(lambda (a)
(list (* (car a) (cadr a))
;;mxy
(* (car a) (car a))
;;mxx
(* (cadr a) (cadr a))
;;myy
(* (car a) (caddr a))
;;mxz
(* (cadr a) (caddr a))
;;myz
(* (caddr a) (caddr a))
;;mzz
)
)
xyz
)
)
)
M_s (mapcar '(lambda(x) (/ x (float n))) M_s)
M_xy (car M_s)
M_xx (cadr M_s)
M_yy (caddr M_s)
M_xz (cadddr M_s)
M_yz (nth 4 M_s)
M_zz (nth 5 M_s)
M_z (+ M_xx M_yy)
cov_xy (- (* M_xx M_yy) (* M_xy M_xy))
var_z (- M_zz (* M_z M_z))
A3 (* 4.0 M_z)
A2 (- (* -3.0 M_z M_z) M_zz)
A1 (+ (* var_z M_z)
(* 4.0 cov_xy M_z)
(* -1.0 M_xz M_xz)
(* -1.0 M_yz M_yz)
)
A0 (+ (* M_xz (- (* M_xz M_yy) (* M_yz M_xy)))
(* M_yz (- (* M_yz M_xx) (* M_xz M_xy)))
(* -1.0 cov_xy var_z)
)
A22 (+ A2 A2)
A33 (+ A3 A3 A3)
mark T
i 0
iter_x 0.0
iter_y A0
)
(while (and mark (< i max_iter))
(setq delta_y (+ A1 (* iter_x (+ A22 (* iter_x A33)))))
(if (< (abs delta_y) 0.00000000001)
(setq mark nil)
(progn
(setq x_new (- iter_x (/ iter_y delta_y)))
(if (< (abs (- x_new iter_x)) 0.00000001)
(setq mark nil)
(progn
(setq y_new
(+ A0
(* x_new (+ A1 (* x_new (+ A2 (* x_new A3)))))
)
)
(if (>= (abs y_new) (abs iter_y))
;;(< (abs(- y_new iter_y)) 0.00000001)
(setq mark nil)
(setq iter_x x_new
iter_y y_new
)
)
)
)
)
)
(setq i (1+ i))
)
;;(setq iter_x(cadr (square_root A33 A22 A1))
;; iter_y (+ A0 (* x_new (+ A2 (* A3 x_new)))))
(setq DET (+ (* iter_x iter_x)
(* -1.0 iter_x M_z)
cov_xy
)
x_c (/ (- (* M_xz (- M_yy iter_x))
(* M_yz M_xy)
)
DET
2.0
)
y_c (/ (- (* M_yz (- M_xx iter_x))
(* M_xz M_xy)
)
DET
2.0
)
rad (sqrt (+ (* x_c x_c) (* y_c y_c) M_z))
x_c (+ x_c (car avg))
y_c (+ y_c (cadr avg))
)
(setq
radian (vl-sort
(mapcar '(lambda (x) (angle (list x_c y_c) x)) pnt_lst)
'(lambda (e1 e2) (< e1 e2))
)
)
(list (list x_c y_c) rad (car radian) (last radian))
)
;;************************************************************************
;;** end taubin无约束的圆弧拟合 **
;;************************************************************************
;;************************************************************************
;;** start taubin一个端点约束的圆弧拟合 **
;;************************************************************************
(defun taubin_circle_fit_one_end(pnt_lst st_pnt max_iter / p_pnt n
xy_to_xyz avg xyz M_s
M_s M_xy M_xx M_yy M_xz
M_yz M_zz M_z cov_xy var_z
A3 A2 A1 A0 A22
A33 mark i iter_x iter_y
delta_y x_new y_new DET x_c
y_c rad radian
)
;;max_iter为特征方程求解最大迭代次数，一般4到6次收敛，可设置10或更大
(defun one_end_xy_to_xyz (xy avg stpnt / z zpnt xy-st)
(setq xy (mapcar '- xy avg)
zpnt (apply '+ (mapcar '* xy xy))
stpnt (mapcar '- stpnt avg)
z (- (apply '+ (mapcar '* xy xy))
(apply '+ (mapcar '* stpnt stpnt))
)
xy-st (mapcar '- xy stpnt)
)
(list (car xy-st) (cadr xy-st) z zpnt)
)
(defun square_root (a b c / sqrt_b2_4ac)
(setq sqrt_b2_4ac (sqrt (- (* b b) (* 4.0 a c))))
(list (/ (+ b sqrt_b2_4ac) -2.0 a)
(/ (- b sqrt_b2_4ac) -2.0 a)
)
)
(defun scale(lst / min_value)
(setq min_value(apply 'min (mapcar 'abs lst)))
(mapcar '(lambda(x) (/ x min_value)) lst)
)
(setq n (length pnt_lst)
avg (mapcar '/
(apply 'mapcar (cons '+ pnt_lst))
(list (float n) (float n))
)
xyz (mapcar '(lambda (a) (one_end_xy_to_xyz a avg stpnt)) pnt_lst)
M_s (apply
'mapcar
(cons '+
(mapcar
'(lambda (a)
(list (* (car a) (cadr a))
;;mxy
(* (car a) (car a))
;;mxx
(* (cadr a) (cadr a))
;;myy
(* (car a) (caddr a))
;;mxz
(* (cadr a) (caddr a))
;;myz
(* (caddr a) (caddr a))
;;mzz
)
)
xyz
)
)
)
p_pnt (mapcar '- stpnt avg)
p_pnt (list (car p_pnt) (cadr p_pnt) (apply '+ (mapcar '* p_pnt p_pnt)))
;p_pnt (mapcar '(lambda(x) (* x n)) p_pnt)
;M_s (mapcar '(lambda(x) (/ x n)) M_s)
;M_s (scale M_s)
M_xy (car M_s)
M_xx (cadr M_s)
M_yy (caddr M_s)
M_xz (- (cadddr M_s) (* 2.0 (car p_pnt)));(cadddr M_s)
M_yz (- (nth 4 M_s) (* 2.0 (cadr p_pnt)));(nth 4 M_s);
M_zz (nth 5 M_s)
;N_z (nth 6 M_s)
M_z (+ M_xx M_yy)
cov_xy (- (* M_xx M_yy) (* M_xy M_xy))
A3 (* 4.0 (caddr p_pnt))
A2 (- (* -4.0 M_z (caddr p_pnt)) M_zz)
A1 (+ (* M_zz M_z)
(* 4.0 cov_xy (caddr p_pnt))
(* -1.0 M_xz M_xz)
(* -1.0 M_yz M_yz)
)
A0 (+ (* M_xz (- (* M_xz M_yy) (* M_yz M_xy)))
(* M_yz (- (* M_yz M_xx) (* M_xz M_xy)))
(* -1.0 cov_xy M_zz)
)
A22 (+ A2 A2)
A33 (+ A3 A3 A3)
mark T
i 0
iter_x 0.0
iter_y A0
)
(while (and mark (< i max_iter))
(setq delta_y (+ A1 (* iter_x (+ A22 (* iter_x A33)))))
(if (< (abs delta_y) 0.00000000001)
(setq mark nil)
(progn
(setq x_new (- iter_x (/ iter_y delta_y)))
(if (< (abs (- x_new iter_x)) 0.00000001)
(setq mark nil)
(progn
(setq y_new
(+ A0
(* x_new (+ A1 (* x_new (+ A2 (* x_new A3)))))
)
)
(if (>= (abs y_new) (abs iter_y))
;;(< (abs(- y_new iter_y)) 0.00000001)
(setq mark nil)
(setq iter_x x_new
iter_y y_new
)
)
)
)
)
)
(setq i (1+ i))
)
;;(setq iter_x(cadr (square_root A33 A22 A1))
;; iter_y (+ A0 (* x_new (+ A2 (* A3 x_new)))))
(setq DET (+ (* iter_x iter_x)
(* -1.0 iter_x M_z)
cov_xy
)
x_c (/ (- (* M_xz (- M_yy iter_x))
(* M_yz M_xy)
)
DET
2.0
)
y_c (/ (- (* M_yz (- M_xx iter_x))
(* M_xz M_xy)
)
DET
2.0
)
x_c (+ x_c (car avg))
y_c (+ y_c (cadr avg))
rad (distance (list x_c y_c) (list (car stpnt) (cadr stpnt)))
)
(setq
radian (vl-sort
(mapcar '(lambda (x) (angle (list x_c y_c) x)) (cons stpnt pnt_lst))
'(lambda (e1 e2) (< e1 e2))
)
)
(list (list x_c y_c) rad (car radian) (last radian))
)
;;************************************************************************
;;** end taubin一个端点约束的圆弧拟合 **
;;************************************************************************
;;************************************************************************
;;** start taubin二个端点约束的圆弧拟合 **
;;************************************************************************
(defun taubin_circle_fit_two_end(pnt_lst stpnt endpnt /
cal_coef_t Normalvector
perpendicular n
avg st_avg en_avg
mid l_p sum
c0 c1 c2
Dc vt cen
rad radian
)
(defun cal_coef_t (st end ipnt _perd mid / midp _ipnt _st z_i
z_a __xyi a0 a1 a2 b0 b1
b2
)
(setq _ipnt (mapcar '- ipnt mid)
_st (mapcar '- st mid)
z_i (apply '+ (mapcar '* _ipnt _ipnt))
z_a (apply '+ (mapcar '* _st _st))
__xyi (apply '+ (mapcar '* _perd (mapcar '- _ipnt _st)))
)
(setq a0(* (- z_i z_a) (- z_i z_a))
a1(* -4.0 __xyi (- z_i z_a))
a2(* 4.0 __xyi __xyi)
b0 z_a
b1 (* -2.0 (apply '+ (mapcar '* _perd _st)))
b2 1.0
)
(list a0 a1 a2 b0 b1 b2)
)
(defun Normalvector(st end / vector len)
(setq vector(mapcar '* (reverse (mapcar '- end st)) '(-1 1))
len(sqrt(apply '+ (mapcar '* vector vector)))
)
(mapcar '(lambda(x) (/ x len)) vector)
)
(defun perpendicular (st end / st-end)
;;Ay = Bx + C
(setq st-end (mapcar '- st end))
(list (cadr st-end)
(* -1.0 (car st-end))
(* 0.5 (apply '+ (mapcar '* st-end (mapcar '+ st end))))
)
)
(setq n (length pnt_lst)
avg (mapcar '/
(apply 'mapcar (cons '+ pnt_lst))
(list (float n)(float n))
)
st_avg(mapcar '- stpnt avg)
en_avg(mapcar '- endpnt avg)
mid (mapcar '/ (mapcar '+ st_avg en_avg) (list 2.0 2.0))
l_p (Normalvector st_avg en_avg)
sum (apply
'mapcar
(cons '+
(mapcar
'(lambda (a)
(cal_coef_t
st_avg
en_avg
(mapcar '- a avg)
l_p
mid
)
)
pnt_lst
)
)
)
sum (mapcar '(lambda(x) (/ x (float n))) sum)
;;scl (apply 'min (mapcar 'abs sum))
;;sum (mapcar '(lambda(x) (/ x scl)) sum)
c0(- (* (nth 1 sum) (nth 3 sum))
(* (nth 0 sum) (nth 4 sum)))
c1(- (* (nth 2 sum) (nth 3 sum) 2.0)
(* (nth 0 sum) (nth 5 sum) 2.0))
c2(- (* (nth 2 sum) (nth 4 sum))
(* (nth 1 sum) (nth 5 sum)))
Dc(- (* c1 c1)
(* 4.0 c0 c2))
)
(setq vt nil)
(if (>= c2 0) (setq sign 1) (setq sign -1))
(cond ((and (= c2 0) (> c1 0)) (setq vt(/ c0 c1 -1.0)))
((and (not(zerop c2)) (< c1 0) (> Dc 0)) (setq vt(/ (- (sqrt Dc) c1) c2 2.0)))
((and (not(zerop c2)) (= c1 0) (> Dc 0)) (setq vt(* (sqrt (/ c0 c2 -1.0)) sign)))
((and (not(zerop c2)) (> c1 0) (> Dc 0)) (setq vt(/ (* 2 c0) (+ (sqrt Dc) c1) -1.0)))
)
;(setq quatic_equation(equation (list c0 c1 c2) (+ cu 0.0)))
;;; (defun equation(pl value)
;;; (apply '+ (mapcar '* pl (list 1 value (* value value)))))
(if vt
(progn
;;; (if (< (abs(car l_p)) 0.0000000001)
;;; (setq cv (cadr st_avg))
;;; (setq cv (/ (apply '+ (mapcar '* (cdr l_p) (list cu 1))) (car l_p)))
;;; )
(setq cen(mapcar '+ (list (* vt (car l_p)) (* vt (cadr l_p))) mid avg)
rad (distance cen (list (car stpnt) (cadr stpnt)))
radian (vl-sort (mapcar '(lambda (x) (angle cen x)) (append (list stpnt endpnt) pnt_lst))
'(lambda (e1 e2) (< e1 e2))
)
)
)
(alert "方程无解，无法拟合成圆弧！")
)
(list cen rad (car radian) (last radian))
)
;;************************************************************************
;;** end taubin二个端点约束的圆弧拟合 **
;;************************************************************************
(defun line_fit (pnt_lst / a b n
avg b_m b_d dif sum
line_param line_point st_pnt
end_pnt
)
;;直线拟合，y方向距离平方和最小
(defun line_point (x_coord param)
(list x_coord
(apply '+ (mapcar '* param (list 0 x_coord 1)))
)
)
(setq n (length pnt_lst)
avg (mapcar '/
(apply 'mapcar (cons '+ pnt_lst))
(list (float n) (float n) (float n))
)
dif (mapcar '(lambda (a) (mapcar '- a avg)) pnt_lst)
sum (apply
'mapcar
(cons '+
(mapcar
'(lambda (a)
(list (* (car a) (cadr a))
(* (car a) (car a))
)
)
dif
)
)
)
)
(if (< (abs (cadr sum)) 0.00000001)
(setq y_sort (vl-sort (mapcar 'cadr pnt_lst)
'(lambda (e1 e2) (< e1 e2))
)
st_pnt (list (car avg) (car y_sort))
end_pnt (list (car avg) (last y_sort))
line_param (list (list 0 -1.0 (car avg)) st_pnt end_pnt)
)
(setq a (apply '/ sum)
b (- (cadr avg) (* (car avg) a))
x_sort (vl-sort (mapcar 'car pnt_lst)
'(lambda (e1 e2) (< e1 e2))
)
st_pnt (line_point (car x_sort) (list -1 a b))
end_pnt (line_point (last x_sort) (list -1 a b))
line_param (list (list -1 a b) st_pnt end_pnt)
)
)
line_param
)
;;(line_fit pnt_lst)
;;(line_deming_fit pnt_lst)
;;(line_deming_fit (list '(1.0 3.0 0.0) '(2.0 6.0 0.0) '(5.0 10.0 0.0) '(2.0 7.0 0.0) '(2.5 5.0 0.0) '(3.0 7.0 0.0)))
(defun line_deming_fit (pnt_lst / a b n
avg b_m b_d line_param
line_point st_pnt end_pnt
)
;;ssr (y-yt)^2+(x - xt)^2 最小
(defun line_point (x_coord param)
(list x_coord
(apply '+ (mapcar '* param (list 0 x_coord 1)))
)
)
(setq n (length pnt_lst)
avg (mapcar '/
(apply 'mapcar (cons '+ pnt_lst))
(list (float n) (float n) (float n))
)
dif (mapcar '(lambda (a) (mapcar '- a avg)) pnt_lst)
sum (apply
'mapcar
(cons '+
(mapcar
'(lambda (a)
(list (* (car a) (car a))
;;SPDxx
(* (car a) (cadr a))
;;SPDxy
(* (cadr a) (cadr a))
;;SPDyy
)
)
dif
)
)
)
sum (mapcar '/
sum
(list (float (- n 1)) (float (- n 1)) (float (- n 1)))
)
)
(if (< (abs (cadr sum)) 0.00000001)
(setq y_sort (vl-sort (mapcar 'cadr pnt_lst)
'(lambda (e1 e2) (< e1 e2))
)
st_pnt (list (car avg) (car y_sort))
end_pnt (list (car avg) (last y_sort))
line_param (list (list 0 1 (car avg)) st_pnt end_pnt)
)
(setq yy-xx (- (caddr sum) (car sum))
xy (cadr sum)
a (/ (+ yy-xx (sqrt (+ (* yy-xx yy-xx) (* 4.0 xy xy))))
2.0
xy
)
b (- (cadr avg) (* (car avg) a))
x_sort (vl-sort (mapcar 'car pnt_lst)
'(lambda (e1 e2) (< e1 e2))
)
st_pnt (line_point (car x_sort) (list -1 a b))
end_pnt (line_point (last x_sort) (list -1 a b))
line_param (list (list -1 a b) st_pnt end_pnt)
)
)
line_param
)
;;(circle_fit (list '(12.0 23.0) '(23.0 32.0) '(11.0 32.0) '(13.0 24.0)))
(defun draw_res (obj pnt_lst h ro threold unit / i avg_res res)
;;计算残差并绘制到点上 h为字高， ro 为旋转
(setq i 0
obj (vlax-ename->vla-object obj)
avg_res 0
)
(foreach pnt pnt_lst
(setq pnt (list (car pnt) (cadr pnt) 0.0)
res (distance (vlax-curve-getclosestpointto obj pnt t) pnt)
res (* (caddr unit) res)
avg_res (+ avg_res res)
text (strcat (rtos res 2 (last unit)) (cadr unit))
)
;;小于指定阈值变绿色，否则红色
(if (< res (* threold (caddr unit)))
(entmakex (list '(0 . "TEXT")
'(8 . "Residuals")
(cons 10 pnt)
(cons 40 h)
(cons 50 ro)
(cons 62 3)
(cons 1 text)
)
)
(entmakex (list '(0 . "TEXT")
'(8 . "Residuals")
(cons 10 pnt)
(cons 40 h)
(cons 50 ro)
(cons 62 1)
(cons 1 text)
)
)
)
)
(/ avg_res (float (length pnt_lst)))
)
(vl-load-com)
(setq CMDECHO (getvar "CMDECHO")
OSMODE (getvar "OSMODE")
)
(setvar "CMDECHO" 0)
(setvar "OSMODE" 0)
(if (not (tblsearch "layer" "Residuals"))
(command "_layer" "m" "Residuals" "c" 8 "" "")
)
(if (not (tblsearch "layer" "fit_line"))
(command "_layer" "m" "fit_line" "c" 8 "" "")
)
(main_fun)
(princ "拟合完成！")
(setvar "CMDECHO" CMDECHO)
(setvar "OSMODE" OSMODE)
)
此处可能存在不合适展示的内容，页面不予展示。您可通过相关编辑功能自查并修改。
如您确认内容无涉及 不当用语 / 纯广告导流 / 暴力 / 低俗色情 / 侵权 / 盗版 / 虚假 / 无价值内容或违法国家有关法律法规的内容，可点击提交进行申诉，我们将尽快为您处理。