公路路线计算 缓和曲线精确计算

2008-04-27 22:17 阅读(?)评论(0)

公路路线计算 缓和曲线精确计算.lsp


;;;===================================================================
;;;功能:计算缓和曲线上一点的坐标值及方位角                           
;;;参数:PT0  ----完整的缓和曲线原点处的坐标                          
;;;      ANG0 ----完整的缓和曲线原点处的方位角                        
;;;      A    ----缓和曲线参数                                        
;;;      L    ----待求点距离原点的长度                                
(defun ZML-LX-HHQX-GETPOINTATDIST (PT0	ANG0 A	  L    /    ZML-!
				   R	X    Y	  I    JD   *TEST*
				   N43	N22  N41  N21  XX   YY	 DIST
				   ANG	PT1  ANG1
				  )
;;;    (princ "\nA=")
;;;    (princ A)
;;;    (princ "     L=")
;;;    (princ L)
    ;;=======================================
    ;;定义  阶乘函数
    ;;返回类型:实数
    (defun ZML-! (INT)
	(if (> INT 1)
	    (* INT (ZML-! (1- INT)))
	    1.0
	)
    )
    ;;=================
    ;;转化为实数
    (setq ANG0 (float ANG0)
	  A    (float A)
	  L    (float L)
    )

    ;;如果在原点,就直接返回
    (if	(= L 0.0)
	(list PT0 ANG0)
	(progn
	    ;;距原点L处的半径值
	    (setq R (/ (* A A) L))
	    (if	(< A 0)
		(setq R (- R))
	    )

	    ;;计算 X Y 坐标
	    (setq X  0
		  Y  0
		  I  1
		  JD 1e-8 ;_预设精度阀值
	    )
	    (setq *TEST* t)
	    (while (and	*TEST*
			(< I 30) ;_做多计算30阶
		   )
		(setq N43 (- (* 4. I) 3)
		      N22 (- (* 2. I) 2)
		      XX  (/ (* (expt -1.0 (1+ I)) (expt L N43))
			     (ZML-! N22)
			     (expt 2.0 N22)
			     N43
			     (expt R N22)
			     (expt L N22)
			  )
		)

		(setq N41 (- (* 4. I) 1)
		      N21 (- (* 2. I) 1)
		      YY  (/ (* (expt -1.0 (1+ I)) (expt L N41))
			     (ZML-! N21)
			     (expt 2.0 N21)
			     N41
			     (expt R N21)
			     (expt L N21)
			  )
		)

		(setq X	(+ X XX)
		      Y	(+ Y YY)
		)

		(if (and (< (abs XX) JD)
			 (< (abs YY) JD)
		    )
		    (setq *TEST* NIL)
		)
		(setq I (1+ I))
	    )

	    (setq DIST (sqrt (+ (* X X) (* Y Y)))
		  ANG  (atan Y X)
	    )
	    (princ "   i:")
	    (princ I)
;;;	    (princ "\nl:")
;;;	    (princ L)
;;;	    (princ "   >>>dist:")
;;;	    (princ DIST)

	    (setq PT1 (polar PT0
			     (+ ANG0 ANG)
			     DIST
		      )
	    )
	    (setq ANG1 (+ ANG0 (/ L 2.0 R)))
	    ;;返回
	    (list PT1 ANG1)
	)
    )
)
;;;=======================================
   阅读(?)评论(0)
 
表  情:
加载中...
 

请各位遵纪守法并注意语言文明