測量程序、測量代碼、方便實用參考模板

1、Function cnx(zk, s, a, d, v) '完整曲線 直線+緩和曲線+圓曲線+緩和曲線+直線 If zk >= 60600 And zk <= 62726.759 Then '有關(guān)數(shù)據(jù)輸入 jd1 = 60600 jd1x = 4654660.879 jd1y = 487662.316 jd2 = 61940.814 jd2x = 4654015.566 jd2y = 486487.006 ls = 315 r = 2400 u = 1 zh = 61122.002 hy = 61437.002 yh = 62411.759 hz = 62726.7

2、59 '有關(guān)數(shù)據(jù)計算 Let pi = 3.14159265358979 yy = jd2y - jd1y xx = jd2x - jd1x fwj = Atn(yy / xx) If yy > 0 And xx < 0 Then fwj = fwj + pi End If If yy < 0 And xx < 0 Then fwj = fwj + pi End If If yy < 0 And xx > 0 Then fwj = fwj + pi * 2 End If If fwj > pi * 2 Then fwj = fwj - pi *

3、 2 End If If fwj < 0 Then fwj = fwj + pi * 2 End If zhx = jd1x + Cos(fwj) * (zh - jd1) zhy = jd1y + Sin(fwj) * (zh - jd1) zhj = fwj + u * Atn(hy - zh) 3 / (6 * r * ls) - (hy - zh) 7 / (336 * r 3 * ls 3) + (hy - zh) 11 / (42240 * r 5 * ls 5) - (hy - zh) 15 / (9676800 * r 7 * ls 7) / (hy - zh) - (h

4、y - zh) 5 / (40 * r 2 * ls 2) + (hy - zh) 9 / (3456 * r 4 * ls 4) - (hy - zh) 13 / (599040 * r 6 * ls 6) + (hy - zh) 17 / (175472640 * r 8 * ls 8) hyfwj = fwj + (hy - zh) 2 / (2 * r * ls) * u zhc = (hy - zh) 3 / (6 * r * ls) - (hy - zh) 7 / (336 * r 3 * ls 3) + (hy - zh) 11 / (42240 * r 5 * ls 5) -

5、(hy - zh) 15 / (9676800 * r 7 * ls 7) / Sin(Atn(hy - zh) 3 / (6 * r * ls) - (hy - zh) 7 / (336 * r 3 * ls 3) + (hy - zh) 11 / (42240 * r 5 * ls 5) - (hy - zh) 15 / (9676800 * r 7 * ls 7) / (hy - zh) - (hy - zh) 5 / (40 * r 2 * ls 2) + (hy - zh) 9 / (3456 * r 4 * ls 4) - (hy - zh) 13 / (599040 * r 6

6、* ls 6) + (hy - zh) 17 / (175472640 * r 8 * ls 8) hyx = zhx + Cos(zhj) * zhc hyy = zhy + Sin(zhj) * zhc hyj = hyfwj + u * (yh - hy) / r) / 2 hyc = Sin(yh - hy) / r) / 2) * 2 * r yhx = hyx + Cos(hyj) * hyc yhy = hyy + Sin(hyj) * hyc yhfwj = hyfwj + u * (yh - hy) / r) hzj = yhfwj + u * (hz - yh) 2 / (

7、2 * r * ls) - Atn(hz - yh) 3 / (6 * r * ls) - (hz - yh) 7 / (336 * r 3 * ls 3) + (hz - yh) 11 / (42240 * r 5 * ls 5) - (hz - yh) 15 / (9676800 * r 7 * ls 7) / (hz - yh) - (hz - yh) 5 / (40 * r 2 * ls 2) + (hz - yh) 9 / (3456 * r 4 * ls 4) - (hz - yh) 13 / (599040 * r 6 * ls 6) + (hz - yh) 17 / (1754

8、72640 * r 8 * ls 8) hzc = (hz - yh) 3 / (6 * r * ls) - (hz - yh) 7 / (336 * r 3 * ls 3) + (hz - yh) 11 / (42240 * r 5 * ls 5) - (hz - yh) 15 / (9676800 * r 7 * ls 7) / Sin(Atn(hz - yh) 3 / (6 * r * ls) - (hz - yh) 7 / (336 * r 3 * ls 3) + (hz - yh) 11 / (42240 * r 5 * ls 5) - (hz - yh) 15 / (9676800

9、 * r 7 * ls 7) / (hz - yh) - (hz - yh) 5 / (40 * r 2 * ls 2) + (hz - yh) 9 / (3456 * r 4 * ls 4) - (hz - yh) 13 / (599040 * r 6 * ls 6) + (hz - yh) 17 / (175472640 * r 8 * ls 8) hzx = yhx + Cos(hzj) * hzc hzy = yhy + Sin(hzj) * hzc hzfwj = yhfwj + u * (hz - yh) 2 / (2 * r * ls) '直線計算 If zk >=

10、 jd1 And zk <= zh And v = 1 Then cnx = jd1x + Cos(fwj) * (zk - jd1) + Cos(fwj) * s + Cos(fwj + a * pi / 180) * d End If If zk >= jd1 And zk <= zh And v = 2 Then cnx = jd1y + Sin(fwj) * (zk - jd1) + Sin(fwj) * s + Sin(fwj + a * pi / 180) * d End If '第一條緩和曲線計算 If zk > zh And zk < hy

11、 Then l = zk - zh x = l - l 5 / (40 * r 2 * ls 2) + l 9 / (3456 * r 4 * ls 4) - l 13 / (599040 * r 6 * ls 6) + l 17 / (175472640 * r 8 * ls 8) y = l 3 / (6 * r * ls) - l 7 / (336 * r 3 * ls 3) + l 11 / (42240 * r 5 * ls 5) - l 15 / (9676800 * r 7 * ls 7) j = Atn(y / x) c = y / Sin(j)1 / 47 zj = l 2

12、/ (2 * r * ls) If zk > zh And zk < hy And v = 1 Then cnx = zhx + Cos(fwj + u * j) * c + Cos(fwj + u * zj) * s + Cos(fwj + u * zj + a * pi / 180) * d End If If zk > zh And zk < hy And v = 2 Then cnx = zhy + Sin(fwj + u * j) * c + Sin(fwj + u * zj) * s + Sin(fwj + u * zj + a * pi / 180) *

13、d End If End If '圓曲線計算 If zk >= hy And zk <= yh Then l = zk - hy j = (l / r) / 2 zj = 2 * j c = Sin(j) * 2 * r If zk >= hy And zk <= yh And v = 1 Then cnx = hyx + Cos(hyfwj + u * j) * c + Cos(hyfwj + u * zj) * s + Cos(hyfwj + u * zj + a * pi / 180) * d End If If zk >= hy And zk &l

14、t;= yh And v = 2 Then cnx = hyy + Sin(hyfwj + u * j) * c + Sin(hyfwj + u * zj) * s + Sin(hyfwj + u * zj + a * pi / 180) * d End If End If '第二條緩和曲線計算 If zk > yh And zk < hz Then l = hz - zk x = l - l 5 / (40 * r 2 * ls 2) + l 9 / (3456 * r 4 * ls 4) - l 13 / (599040 * r 6 * ls 6) + l 17 / (

15、175472640 * r 8 * ls 8) y = l 3 / (6 * r * ls) - l 7 / (336 * r 3 * ls 3) + l 11 / (42240 * r 5 * ls 5) - l 15 / (9676800 * r 7 * ls 7) j = Atn(y / x) c = y / Sin(j) zj = l 2 / (2 * r * ls) fj = zj - j If zk > yh And zk < hz And v = 1 Then cnx = hzx + Cos(hzfwj + pi + (-u) * j) * c + Cos(hzfwj

16、 + (-u) * zj) * s + Cos(hzfwj + (-u) * zj + a * pi / 180) * d End If If zk > yh And zk < hz And v = 2 Then cnx = hzy + Sin(hzfwj + pi + (-u) * j) * c + Sin(hzfwj + (-u) * zj) * s + Sin(hzfwj + (-u) * zj + a * pi / 180) * d End If End If End If '結(jié)束 '完整曲線 直線+緩和曲線+圓曲線+緩和曲線+直線 If zk >=

17、62726.759 And zk <= 64108.131 Then '有關(guān)數(shù)據(jù)輸入 jd1 = 62726.759 '交點1的樁號 jd1x = 4654044.448 '交點1的X坐標 jd1y = 485668.703 '交點1的Y坐標 jd2 = 63645.249 '交點2的樁號 jd2x = 4654076.846 '交點2的X坐標 jd2y = 484750.785 '交點2的Y坐標 ls = 335 '緩和曲線長 r = 3000 '圓曲線半徑 u = -1 '轉(zhuǎn)向（左轉(zhuǎn)為負，右轉(zhuǎn)為正） zh

18、 = 63180.155 '直緩點樁號 hy = 63515.155 '緩圓點樁號 yh = 63773.132 '圓緩點樁號 hz = 64108.131 '緩直點樁號 '有關(guān)數(shù)據(jù)計算 Let pi = 3.14159265358979 yy = jd2y - jd1y xx = jd2x - jd1x wj = Atn(yy / xx) If yy > 0 And xx > 0 Then fwj = wj End If If yy > 0 And xx < 0 Then fwj = wj + pi End If If yy &

19、lt; 0 And xx < 0 Then fwj = wj + pi End If If yy < 0 And xx > 0 Then fwj = wj + pi * 2 End If If fwj > pi * 2 Then fwj = fwj - pi * 2 End If If fwj < 0 Then fwj = fwj + pi * 2 End If zhx = jd1x + Cos(fwj) * (zh - jd1) zhy = jd1y + Sin(fwj) * (zh - jd1) zhj = fwj + u * Atn(hy - zh) 3 /

20、 (6 * r * ls) - (hy - zh) 7 / (336 * r 3 * ls 3) + (hy - zh) 11 / (42240 * r 5 * ls 5) - (hy - zh) 15 / (9676800 * r 7 * ls 7) / (hy - zh) - (hy - zh) 5 / (40 * r 2 * ls 2) + (hy - zh) 9 / (3456 * r 4 * ls 4) - (hy - zh) 13 / (599040 * r 6 * ls 6) + (hy - zh) 17 / (175472640 * r 8 * ls 8) hyfwj = fw

21、j + (hy - zh) 2 / (2 * r * ls) * u zhc = (hy - zh) 3 / (6 * r * ls) - (hy - zh) 7 / (336 * r 3 * ls 3) + (hy - zh) 11 / (42240 * r 5 * ls 5) - (hy - zh) 15 / (9676800 * r 7 * ls 7) / Sin(Atn(hy - zh) 3 / (6 * r * ls) - (hy - zh) 7 / (336 * r 3 * ls 3) + (hy - zh) 11 / (42240 * r 5 * ls 5) - (hy - zh

22、) 15 / (9676800 * r 7 * ls 7) / (hy - zh) - (hy - zh) 5 / (40 * r 2 * ls 2) + (hy - zh) 9 / (3456 * r 4 * ls 4) - (hy - zh) 13 / (599040 * r 6 * ls 6) + (hy - zh) 17 / (175472640 * r 8 * ls 8) hyx = zhx + Cos(zhj) * zhc hyy = zhy + Sin(zhj) * zhc hyj = hyfwj + u * (yh - hy) / r) / 2 hyc = Sin(yh - h

23、y) / r) / 2) * 2 * r yhx = hyx + Cos(hyj) * hyc yhy = hyy + Sin(hyj) * hyc yhfwj = hyfwj + u * (yh - hy) / r) hzj = yhfwj + u * (hz - yh) 2 / (2 * r * ls) - Atn(hz - yh) 3 / (6 * r * ls) - (hz - yh) 7 / (336 * r 3 * ls 3) + (hz - yh) 11 / (42240 * r 5 * ls 5) - (hz - yh) 15 / (9676800 * r 7 * ls 7)

24、/ (hz - yh) - (hz - yh) 5 / (40 * r 2 * ls 2) + (hz - yh) 9 / (3456 * r 4 * ls 4) - (hz - yh) 13 / (599040 * r 6 * ls 6) + (hz - yh) 17 / (175472640 * r 8 * ls 8) hzc = (hz - yh) 3 / (6 * r * ls) - (hz - yh) 7 / (336 * r 3 * ls 3) + (hz - yh) 11 / (42240 * r 5 * ls 5) - (hz - yh) 15 / (9676800 * r 7

25、 * ls 7) / Sin(Atn(hz - yh) 3 / (6 * r * ls) - (hz - yh) 7 / (336 * r 3 * ls 3) + (hz - yh) 11 / (42240 * r 5 * ls 5) - (hz - yh) 15 / (9676800 * r 7 * ls 7) / (hz - yh) - (hz - yh) 5 / (40 * r 2 * ls 2) + (hz - yh) 9 / (3456 * r 4 * ls 4) - (hz - yh) 13 / (599040 * r 6 * ls 6) + (hz - yh) 17 / (175

26、472640 * r 8 * ls 8) hzx = yhx + Cos(hzj) * hzc hzy = yhy + Sin(hzj) * hzc hzfwj = yhfwj + u * (hz - yh) 2 / (2 * r * ls) '直線計算 If zk >= jd1 And zk <= zh And v = 1 Then cnx = jd1x + Cos(fwj) * (zk - jd1) + Cos(fwj) * s + Cos(fwj + a * pi / 180) * d End If If zk >= jd1 And zk <= zh An

27、d v = 2 Then cnx = jd1y + Sin(fwj) * (zk - jd1) + Sin(fwj) * s + Sin(fwj + a * pi / 180) * d End If '第一條緩和曲線計算 If zk > zh And zk < hy Then l = zk - zh x = l - l 5 / (40 * r 2 * ls 2) + l 9 / (3456 * r 4 * ls 4) - l 13 / (599040 * r 6 * ls 6) + l 17 / (175472640 * r 8 * ls 8) y = l 3 / (6 *

28、 r * ls) - l 7 / (336 * r 3 * ls 3) + l 11 / (42240 * r 5 * ls 5) - l 15 / (9676800 * r 7 * ls 7) j = Atn(y / x) c = y / Sin(j) zj = l 2 / (2 * r * ls) If zk > zh And zk < hy And v = 1 Then cnx = zhx + Cos(fwj + u * j) * c + Cos(fwj + u * zj) * s + Cos(fwj + u * zj + a * pi / 180) * d End If I

29、f zk > zh And zk < hy And v = 2 Then cnx = zhy + Sin(fwj + u * j) * c + Sin(fwj + u * zj) * s + Sin(fwj + u * zj + a * pi / 180) * d End If End If '圓曲線計算 If zk >= hy And zk <= yh Then l = zk - hy j = (l / r) / 2 zj = 2 * j c = Sin(j) * 2 * r If zk >= hy And zk <= yh And v = 1 T

30、hen cnx = hyx + Cos(hyfwj + u * j) * c + Cos(hyfwj + u * zj) * s + Cos(hyfwj + u * zj + a * pi / 180) * d End If If zk >= hy And zk <= yh And v = 2 Then cnx = hyy + Sin(hyfwj + u * j) * c + Sin(hyfwj + u * zj) * s + Sin(hyfwj + u * zj + a * pi / 180) * d End If End If '第二條緩和曲線計算 If zk >

31、 yh And zk < hz Then l = hz - zk x = l - l 5 / (40 * r 2 * ls 2) + l 9 / (3456 * r 4 * ls 4) - l 13 / (599040 * r 6 * ls 6) + l 17 / (175472640 * r 8 * ls 8) y = l 3 / (6 * r * ls) - l 7 / (336 * r 3 * ls 3) + l 11 / (42240 * r 5 * ls 5) - l 15 / (9676800 * r 7 * ls 7) j = Atn(y / x) c = y / Sin(

32、j) zj = l 2 / (2 * r * ls) fj = zj - j If zk > yh And zk < hz And v = 1 Then cnx = hzx + Cos(hzfwj + pi + (-u) * j) * c + Cos(hzfwj + (-u) * zj) * s + Cos(hzfwj + (-u) * zj + a * pi / 180) * d End If If zk > yh And zk < hz And v = 2 Then cnx = hzy + Sin(hzfwj + pi + (-u) * j) * c + Sin(h

33、zfwj + (-u) * zj) * s + Sin(hzfwj + (-u) * zj + a * pi / 180) * d End If End If End If '結(jié)束 '完整曲線 直線+緩和曲線+圓曲線+緩和曲線+直線 If zk >= 64108.131 And zk <= 65636.38 Then '有關(guān)數(shù)據(jù)輸入 jd1 = 64108.131 jd1x = 4654001.656 jd1y = 484291.808 jd2 = 65069.288 jd2x = 4653846.269 jd2y = 483343.295 ls = 305

34、 r = 2000 u = 1 zh = 64488.737 hy = 64793.737 yh = 65331.38 hz = 65636.38 '有關(guān)數(shù)據(jù)計算 Let pi = 3.14159265358979 yy = jd2y - jd1y xx = jd2x - jd1x wj = Atn(yy / xx) If yy > 0 And xx > 0 Then fwj = wj End If If yy > 0 And xx < 0 Then fwj = wj + pi End If If yy < 0 And xx < 0 Then fwj

35、 = wj + pi End If If yy < 0 And xx > 0 Then fwj = wj + pi * 2 End If If fwj > pi * 2 Then fwj = fwj - pi * 2 End If If fwj < 0 Then fwj = fwj + pi * 2 End If zhx = jd1x + Cos(fwj) * (zh - jd1) zhy = jd1y + Sin(fwj) * (zh - jd1) zhj = fwj + u * Atn(hy - zh) 3 / (6 * r * ls) - (hy - zh) 7

36、/ (336 * r 3 * ls 3) + (hy - zh) 11 / (42240 * r 5 * ls 5) - (hy - zh) 15 / (9676800 * r 7 * ls 7) / (hy - zh) - (hy - zh) 5 / (40 * r 2 * ls 2) + (hy - zh) 9 / (3456 * r 4 * ls 4) - (hy - zh) 13 / (599040 * r 6 * ls 6) + (hy - zh) 17 / (175472640 * r 8 * ls 8) hyfwj = fwj + (hy - zh) 2 / (2 * r * l

37、s) * u zhc = (hy - zh) 3 / (6 * r * ls) - (hy - zh) 7 / (336 * r 3 * ls 3) + (hy - zh) 11 / (42240 * r 5 * ls 5) - (hy - zh) 15 / (9676800 * r 7 * ls 7) / Sin(Atn(hy - zh) 3 / (6 * r * ls) - (hy - zh) 7 / (336 * r 3 * ls 3) + (hy - zh) 11 / (42240 * r 5 * ls 5) - (hy - zh) 15 / (9676800 * r 7 * ls 7

38、) / (hy - zh) - (hy - zh) 5 / (40 * r 2 * ls 2) + (hy - zh) 9 / (3456 * r 4 * ls 4) - (hy - zh) 13 / (599040 * r 6 * ls 6) + (hy - zh) 17 / (175472640 * r 8 * ls 8) hyx = zhx + Cos(zhj) * zhc hyy = zhy + Sin(zhj) * zhc hyj = hyfwj + u * (yh - hy) / r) / 2 hyc = Sin(yh - hy) / r) / 2) * 2 * r yhx = h

39、yx + Cos(hyj) * hyc yhy = hyy + Sin(hyj) * hyc yhfwj = hyfwj + u * (yh - hy) / r) hzj = yhfwj + u * (hz - yh) 2 / (2 * r * ls) - Atn(hz - yh) 3 / (6 * r * ls) - (hz - yh) 7 / (336 * r 3 * ls 3) + (hz - yh) 11 / (42240 * r 5 * ls 5) - (hz - yh) 15 / (9676800 * r 7 * ls 7) / (hz - yh) - (hz - yh) 5 /

40、(40 * r 2 * ls 2) + (hz - yh) 9 / (3456 * r 4 * ls 4) - (hz - yh) 13 / (599040 * r 6 * ls 6) + (hz - yh) 17 / (175472640 * r 8 * ls 8) hzc = (hz - yh) 3 / (6 * r * ls) - (hz - yh) 7 / (336 * r 3 * ls 3) + (hz - yh) 11 / (42240 * r 5 * ls 5) - (hz - yh) 15 / (9676800 * r 7 * ls 7) / Sin(Atn(hz - yh)

41、3 / (6 * r * ls) - (hz - yh) 7 / (336 * r 3 * ls 3) + (hz - yh) 11 / (42240 * r 5 * ls 5) - (hz - yh) 15 / (9676800 * r 7 * ls 7) / (hz - yh) - (hz - yh) 5 / (40 * r 2 * ls 2) + (hz - yh) 9 / (3456 * r 4 * ls 4) - (hz - yh) 13 / (599040 * r 6 * ls 6) + (hz - yh) 17 / (175472640 * r 8 * ls 8) hzx = y

42、hx + Cos(hzj) * hzc hzy = yhy + Sin(hzj) * hzc hzfwj = yhfwj + u * (hz - yh) 2 / (2 * r * ls) '直線計算 If zk >= jd1 And zk <= zh And v = 1 Then cnx = jd1x + Cos(fwj) * (zk - jd1) + Cos(fwj) * s + Cos(fwj + a * pi / 180) * d End If If zk >= jd1 And zk <= zh And v = 2 Then cnx = jd1y + Si

43、n(fwj) * (zk - jd1) + Sin(fwj) * s + Sin(fwj + a * pi / 180) * d End If '第一條緩和曲線計算 If zk > zh And zk < hy Then l = zk - zh x = l - l 5 / (40 * r 2 * ls 2) + l 9 / (3456 * r 4 * ls 4) - l 13 / (599040 * r 6 * ls 6) + l 17 / (175472640 * r 8 * ls 8) y = l 3 / (6 * r * ls) - l 7 / (336 * r 3

44、* ls 3) + l 11 / (42240 * r 5 * ls 5) - l 15 / (9676800 * r 7 * ls 7) j = Atn(y / x) c = y / Sin(j) zj = l 2 / (2 * r * ls) If zk > zh And zk < hy And v = 1 Then cnx = zhx + Cos(fwj + u * j) * c + Cos(fwj + u * zj) * s + Cos(fwj + u * zj + a * pi / 180) * d End If If zk > zh And zk < hy

45、And v = 2 Then cnx = zhy + Sin(fwj + u * j) * c + Sin(fwj + u * zj) * s + Sin(fwj + u * zj + a * pi / 180) * d End If End If '圓曲線計算 If zk >= hy And zk <= yh Then l = zk - hy j = (l / r) / 2 zj = 2 * j c = Sin(j) * 2 * r If zk >= hy And zk <= yh And v = 1 Then cnx = hyx + Cos(hyfwj +

46、u * j) * c + Cos(hyfwj + u * zj) * s + Cos(hyfwj + u * zj + a * pi / 180) * d End If If zk >= hy And zk <= yh And v = 2 Then cnx = hyy + Sin(hyfwj + u * j) * c + Sin(hyfwj + u * zj) * s + Sin(hyfwj + u * zj + a * pi / 180) * d End If End If '第二條緩和曲線計算 If zk > yh And zk < hz Then l =

47、hz - zk x = l - l 5 / (40 * r 2 * ls 2) + l 9 / (3456 * r 4 * ls 4) - l 13 / (599040 * r 6 * ls 6) + l 17 / (175472640 * r 8 * ls 8) y = l 3 / (6 * r * ls) - l 7 / (336 * r 3 * ls 3) + l 11 / (42240 * r 5 * ls 5) - l 15 / (9676800 * r 7 * ls 7) j = Atn(y / x) c = y / Sin(j) zj = l 2 / (2 * r * ls) f

48、j = zj - j If zk > yh And zk < hz And v = 1 Then cnx = hzx + Cos(hzfwj + pi + (-u) * j) * c + Cos(hzfwj + (-u) * zj) * s + Cos(hzfwj + (-u) * zj + a * pi / 180) * d End If If zk > yh And zk < hz And v = 2 Then cnx = hzy + Sin(hzfwj + pi + (-u) * j) * c + Sin(hzfwj + (-u) * zj) * s + Sin(

49、hzfwj + (-u) * zj + a * pi / 180) * d End If End If End If '結(jié)束 '完整曲線 直線+緩和曲線+圓曲線+緩和曲線+直線 If zk >= 65636.38 And zk <= 67988.769 Then '有關(guān)數(shù)據(jù)輸入 jd1 = 65636.38 jd1x = 4653994.925 jd1y = 482782.098 jd2 = 67179.037 jd2x = 4654389.935 jd2y = 481290.871 ls = 270 r = 1430 u = -1 zh = 66209.0

50、72 hy = 66479.072 yh = 67718.769 hz = 67988.769 '有關(guān)數(shù)據(jù)計算 Let pi = 3.14159265358979 yy = jd2y - jd1y xx = jd2x - jd1x wj = Atn(yy / xx) If yy > 0 And xx > 0 Then fwj = wj End If If yy > 0 And xx < 0 Then fwj = wj + pi End If If yy < 0 And xx < 0 Then fwj = wj + pi End If If yy < 0 And xx > 0 Then fwj = wj + pi * 2 End If If fwj > pi * 2 Then fwj = fwj - pi * 2 End If If fwj < 0 Then fwj = fwj + pi * 2 End If zhx = jd1x + Cos(fwj) * (zh - jd1) zhy = jd1y + Sin(fwj) * (zh - jd1) zhj = fwj + u * Atn(hy - zh) 3 / (6 * r * ls) - (hy - zh) 7 / (336 * r 3