24 A study of indoor location estimation method for navigation. 1130332 2013 2 8
,.,., GPS., GPS,.,,.,.., TOA(Time Of Arrival), TDOA(Time Difference Of Arrival) 2., 3, 3 4., TOA-UMI(Time Of Arrival Unuse Mutual Infase).,, 3., LAN., OS CPU 1., 2,,,,., i
.,, ii
Abstract A study of indoor location estimation method for navigation. Kai Kyogoku In recent years, ubiquitous network which makes everyone to access the information from anywhare. In the ubiquitous network, new services are expected to be provided by using user s location information. Location information in outdoor can be acquired with GPS. However, there is a problem in an indoor environment where GPS satellite waves are not received, and the position estimation lacks it s accuracy. Unfortunately, there is not a indoor reliable location estimation method. From this reason, a new indoor location estimation method is expected. In this paper, a new indoor location estimation method is proposed. As the location estimation method which is most appropriate for indoor navigation, the attainment time method is introduced. The arrival time methods are classified in a couple of types of TOA(Time Of Arrival)method and TDOA(Time Difference Of Arrival)method. The former finds out the location of mobile terminal from the three base stations by synchronizing the time accurately. The latter finds out the mobile terminal location from four base stations. In this paper, the mobile position is estimated from the overlapping of the spheres which are formed virtually using wave arrival times. A new indoor estimation method TOA-UMI(Time Of Arrival Unuse Mutual Inphase) by using three base stations is proposed in this article. It does not need to synchronize the times of three base stations and terminals. Besides, Location estimation method that is most iii
appropriate for indoor navigation is also shown that it would be realized using the wireless local access network technology. An actual proof experiment using two terminals which are assumed as the current and the future turminals. In both cases, the proposed scheme is proved to satisfy the indoor navigation requirements, which are the accuracy, the fast calculating, and the couerage. These results reveal that the proposed scheme is appropriate for the indoor navigation method. key words navigation, indoor use, ubiquitous network, TOA-UMI iv
1 1 1.1.......... 1 1.2................................... 1 1.2.1 ID.............................. 2 1.2.2............................. 3 1.2.3............................. 4 1.2.4............................. 4 TOA.................................. 5 TDOA................................. 5 1.2.5................... 7 1.2.6................................ 7 1.3.................................. 9 2 11 2.1................................ 11 2.2....................... 11 2.3........................ 13 2.3.1....................... 13 2.4................................... 14 2.4.1..................... 14 2.4.2................ 15 2.5................................ 17 2.5.1......................... 17 2.5.2....................... 18 v
3 21 3.1 PC............................. 21 3.2................................... 21 3.3................................... 22 3.4................................... 22 3.4.1........................... 22 3.4.2........................... 25 3.4.3................................ 25 3.4.4.................................. 26 4 27 4.1.................................... 27 4.2.................................. 28 29 31 A 33 vi
1.1 PHS............... 2 1.2 ID............................. 3 1.3 AOA................................. 4 1.4 TOA................................. 5 1.5 TDOA................................ 6 2.1.............................. 15 2.2....................... 16 2.3 TOA........................... 17 2.4 TOA-UMI.............................. 19 3.1 F........................... 23 3.2 P........................... 24 vii
2.1............................... 13 3.1 F................................. 21 3.2 P................................. 22 3.3 F................................ 23 3.4 P................................ 24 ix
1 1.1,., 23, PHS, 1 3,678.7., [1]. PHS 1.1.,., GPS., GPS., 4,.,. 1.2, ID,,, 4., 4, 2. 1
1 1.1 PHS 1.2.1 ID ID,,. ID 1.2.,,,.,,.,.,,.,,,. 2
1.2 1.2 ID 1.2.2 (AOA Angle Of Arrival),,,. AOA 1.3. A B. 2,.,,,.,.,, AOA. 3
1 1.3 AOA 1.2.3 (RSSI Received Signal Strength Indication),.,,.,,.,,. 1.2.4,., 3 10 8 [m/s],,.,,., 30cm., 2. 2. 4
1.2 TOA (TOA Time Of Arrival),,. TOA 1.4. 1.4 TOA TOA, 3,, 3. TDOA (TDOA Time Difference Of Arrival),., 2. TDOA 1.5 [2]. 3, 2. 2 5
1 1.5 TDOA 3,. 3 4, TDOA 3 [3].,,. TOA, TDOA.,., TDOA 3, 4, TOA 6
1.2. 1.2.5, [6]. 1. 0.78,.,. 5 4, 1.6(m/s)[4]., 1.25m[5]., 0.78125. 2. 0.625m., 1. 1.25m[5].,, 0.625m. 3..,. 1.2.6,., 4, 3., 4,,. 1. ID 7
1 1.25m, 1.25m.,,. 2.,.,. 3.,,,.,,.,. 4., 30cm., 1.25m., ID. 2, ID.,, ID., ID.,., ID. 3,.,., TOA, TDOA 2., 3, 3 4., 8
1.3, 3. 1.3 4 2 3. 4 9
2 2.1,, 3 TOA-UMI(Time Of Arrival Unuse Mutual Inphase). 2.2,.,., ZigBee, BlueTooth, UWB(Ultra Wide Band), LAN 4. 2.1.,. 1. ZigBee ZigBee, hop 65,533,., 250kbps 70m,..,. 2. BlueTooth BlueTooth, hop 7, 1 1 11
2. 24Mbps,., 3., 7,,.,. 3. UWB UWB, 10m,., 480Mbps., 1,., 10m. 4. LAN LAN,., 100m,., ZigBee, UWB, LAN. 3., LAN ZigBee. Zigbee LAN., Zigbee,., LAN., UWB LAN., 3. 10m UWB LAN., LAN, 1.2.6. 12
2.3 2.1 ZigBee BlueTooth UWB LAN 250kbps 24Mbps 480Mbps 11.54Mbps hop 65533 7 32 70m 1 100m 10m 100m IEEE 802.15.4 IEEE 802.15.1 IEEE802.15.3a IEEE 802.11 60mW 120mW 100mW 1,000mW 2.4GHz 2.4GHz 3.1G-10.6GHz 2.4GHz, 5GHz NW ( ) 2.3 2.3.1, A, B, C.,. A, B, C,.,. A, B, C 3..,.,.,., 3, 13
2.. LAN, 1. 2.4,.,.,,.,.,.,,.,.,,. 2.4.1,.,,., T.,,.. 2.1.,., r 1, r 2, r 3. 14
2.4 2.1 2.4.2 2.2..,.,.,.,., A, B, C. A, B, C,. 15
2 2.2 16
2.5 2.5 2.5.1 TOA-UMI TOA., TOA 2.3. A, B, C A, B, C, T T, r 1, r 2, r 3 A, B, 2.3 TOA C T., 2.3, 3 3, A(x a, y a, z a ), B(x b, y b, z b ), C(x c, y c, z c )., T T (x, y, z)., T., T. 2.1. D = ct (2.1) D = [m] 17
2 c = ( 3 10 8 )[sec] t = [sec], 2.2, 2.3, 2.4. r 1 = (x x a ) 2 + (y y a ) 2 + (z z a ) 2 (2.2) r 2 = (x x b ) 2 + (y y b ) 2 + (z z b ) 2 (2.3) r 3 = (x x c ) 2 + (y y c ) 2 + (z z c ) 2 (2.4), A(x a, y a, z a ), B(x b, y b, z b ), C(x c, y c, z c ), r 1, r 2, r 3 T (x, y, z), 2.2, 2.3, 2.4 3. 2.5.2, TOA-UMI(Time Of Arrival Unuse Mutual Inphase) 2.4., A, B, C., A,B,C A(x a, y a, z a ), B(x b, y b, z b ), C(x c, y c, z c ),.,,., 2.5. D = ct + n (2.5),.,, r 1, r 2, r 3., r 1, r 2, r 3 T.,, T. 2.4, x,y 2,,,,,., 18
2.5 2.4 TOA-UMI 6, 6,,., A B. A, B, A B,. (x x 1 ) 2 + (y y 1 ) 2 = r1 2 (x x 2 ) 2 + (y y 2 ) 2 = r2 2 (x 1 x 2 )x + (y 1 y 2 )y (x 1 x 2 ) 2 + (y 1 y 2 ) 2 + r 2 1 r 2 2 2 = 0 19
2, (x 1 x 2 ) 2 +(y 1 y 2 ) 2 +r 2 1 r2 2 2 = a, (x 1 x 2 )x + (y 1 y 2 )y a = 0., 2.,,., 2 (2.6), (2.7). x = a(x 1 x 2 ) ± (y 1 y 2 ) ((x 1 x 2 ) + (y 1 y 2 ))r 2 1 a2 (x x 2 ) 2 + (y y 2 ) 2 (2.6) y = a(y 1 y 2 ) (x 1 x 2 ) ((x 1 x 2 ) + (y 1 y 2 ))r 2 1 a2 (x x 2 ) 2 + (y y 2 ) 2 (2.7), A, B x, y 2. n., 2. 2, 3, C. A, C, B, B, C A.,, x, z,,,., r1,r2,r3,,, T(x, y), T(x, y, z). r 1, r 2, r 3,.,,.,,, x, y,,, z. (x, y, z).,,. 20
3,,. 3.1 PC 2., P, F. F 3.1, P 3.2,., F P. F,,. P,. 3.2,., Java., r 1, r 2, r 3,.,.,,.,, 3.1 F OS CPU Windows 7 64bit Intel(R)Core(TM)i7 920@2.67GHz 2.67GHz 21
3 3.2 P OS CPU Windows 7 32bit Intel(R)Core(TM)2 Duo CPU@1.20GHz 1.20GHz., LAN 10 100m. 3.3, 2. F 3.3, P 3.4,., F 3.1, P 3.2. 3.3, 3.4,, 0.01, 0.001, 0.0001.,, 10m, 20m, 30m, 40m, 50m, 60m, 70m, 80m, 90m, 100m 10,., 3.1, 3.2. 3.4 F, P. 3.4.1 F, P 22
3.4 3.3 F m/m 0.01 0.001 0.0001 10 0.014163508 0.100376987 0.129293038 20 0.018750521 0.102976987 0.162084565 30 0.022305352 0.105577981 0.195031638 40 0.027864831 0.111460388 0.236009265 50 0.034975955 0.113942614 0.262176667 60 0.040731977 0.115127985 0.294685835 70 0.046785301 0.118690607 0.327828682 80 0.053009116 0.121568596 0.361584518 90 0.057433301 0.124844262 0.396256776 100 0.060882008 0.129767722 0.431804076 3.1 F 23
3 3.4 P m/m 0.01 0.001 0.0001 10 0.036168778 0.042424985 0.210736076 20 0.069366551 0.054240028 0.5373245203 30 0.05072123692 0.06466324 0.772170089 40 0.060025972 0.086257575 0.942429362 50 0.062178304 0.119497992 1.260475677 60 0.0614166028 0.13250023 1.504459675 70 0.07460897 0.150152613 1.814265452 80 0.088428286 0.170243908 2.041010673 90 0.09095476 0.21810509 2.160682451 100 0.099572493 0.243019392 2.474154037 3.2 P 24
3.4, 1.25m. 3.3, 0.01m, 0.625m. 3.4.2 F 3.3, 0.0001m, LAN 100m, 0.43., 0.78. P 3.4, 0.0001m, LAN 100m 2.47., 0.0001m., 0.001m, 100m 0.25.,. 3.4.3 F, P P F., F P,,,,., TOA-UMI. 25
3 3.4.4, r 1, r 2, r 3,.,,.,.,,,.,.,. 26
4 4.1,,, 3 TOA-UMI.,,., F P.,,., F 0.0001m, 100m 0.431804076., B 0.0001m, 100m 2.474154037., 0.001m, 100m 0.243019392., 2.,. 27
4 4.2,.,,., GPS,.,.,,, [6].,,. 28
,,., 2,, 1,,,,, 29
[1],http://www.soumu.go.jp/menu_news/ s-news/01kiban03_02000167.html(2013/2/1 ) [2] CISCO,https://www.cisco.com/web/JP/solution/ netsol/designzone/literature/pdf/ol-11612-01-j.pdf(2013/2/4 ) [3] Three-dimensional Hyperbolic Positioning System Hindawi,http://www.hindawi.com/journals/vlsi/2002/935925/abs/(2013/2/1 ) [4] 112, 5 4 [5], 10, p.153 [6] PAN,, 24 p.176 31
A Java. import java.math.bigdecimal; import java.lang.math; import java.lang.string; import java.lang.system; import java.math.roundingmode; public class TOAWS { public static void main(string[] args) { double xt,yt,zt,x,y,x1,x2,x3,y1,y2,y3,r1,r2,r3,r4,r5,r6,a,b,c,d, n,z,z1,z2,h,xa,xb,xc,ya,yb,yc,za,zb,zc,alphax,betax,gammax, alphay,betay,gammay,alphaz,betaz,gammaz,seido,seidohoji, jikanxt,jikanyt,jikanzt; int i; // seido = 0.1; seidohoji = seido; // xt = 2;//Math.round (Math.random () * 19) + 1; yt = 2;//Math.round (Math.random () * 19) + 1; 33
A zt = 2;//Math.round (Math.random () * 19) + 1; // ( ) n = 50;//Math.random() + Math.random() + Math.random(); // x1 = 20; y1 = 1; x2 = 10; y2 = 20; z1 = 1; z2 = 20; // jikanxt = xt / 299792458; jikanyt = yt / 299792458; jikanzt = zt / 299792458; // long start = System.nanoTime(); // xt = 299792458 * jakanxt; yt = 299792458 * jakanyt; zt = 299792458 * jakanzt; //x,y r1 = Math.sqrt((0-xt)*(0-xt)+(0-yt)*(0-yt))+n; r2 = Math.sqrt((x1-xt)*(x1-xt)+(y1-yt)*(y1-yt))+n; r3 = Math.sqrt((x2-xt)*(x2-xt)+(y2-yt)*(y2-yt))+n; //,, 34
//A,B a = (Math.pow(x1,2) + Math.pow(y1,2) + Math.pow(r1,2) - Math.pow(r2,2) )/2; xa = (a*x1 + y1 * Math.sqrt((x1*x1 + y1*y1)*r1*r1 - a*a))/(x1*x1 + y1*y1); ya = (a*y1 - x1 * Math.sqrt((x1*x1 + y1*y1)*r1*r1 - a*a))/(x1*x1 + y1*y1); xb = (a*x1 - y1 * Math.sqrt((x1*x1 + y1*y1)*r1*r1 - a*a))/(x1*x1 + y1*y1); yb = (a*y1 + x1 * Math.sqrt((x1*x1 + y1*y1)*r1*r1 - a*a))/(x1*x1 + y1*y1); //A,B C (alphax, alphay) if (Math.abs(ya - y2) + Math.abs(xa - x2) <= Math.abs(yb - y2)+math.abs(xb - x2)){ alphax = xa; alphay = ya; } else { alphax = xb; alphay = yb; } //A,C a = (Math.pow(x2,2) + Math.pow(y2,2) + Math.pow(r1,2) - Math.pow(r3,2))/2; xa = (a*x2 + y2 * Math.sqrt((x2*x2 + y2*y2)*r1*r1 - a*a))/(x2*x2 + y2*y2); ya = (a*y2 - x2 * Math.sqrt((x2*x2 + y2*y2)*r1*r1 - a*a))/(x2*x2 + y2*y2); xb = (a*x2 - y2 * Math.sqrt((x2*x2 + y2*y2)*r1*r1 - a*a))/(x2*x2 + y2*y2); yb = (a*y2 + x2 * Math.sqrt((x2*x2 + y2*y2)*r1*r1 - a*a))/(x2*x2 + y2*y2); //A,C B (betax, betay) if (Math.abs(xa - x1)+math.abs(ya - y1) <= Math.abs(xb - x1)+math.abs(yb - y1)) { betax = xa; betay = ya; 35
A } else { betax = xb; betay = yb; } //C,B a = ((x1-x2)*(x1-x2) + (y1-y2)*(y1-y2) + r3*r3 - r2*r2)/2; xa = (a*(x1-x2) + (y1-y2) * Math.sqrt(((x1-x2)*(x1-x2) + (y1-y2)*(y1-y2))*r3*r3 - a*a))/((x1-x2)*(x1-x2) + (y1-y2)*(y1-y2))+x2; ya = (a*(y1-y2) - (x1-x2) * Math.sqrt(((x1-x2)*(x1-x2) + (y1-y2)*(y1-y2))*r3*r3 - a*a))/((x1-x2)*(x1-x2) + (y1-y2)*(y1-y2))+y2; xb = (a*(x1-x2) - (y1-y2) * Math.sqrt(((x1-x2)*(x1-x2) + (y1-y2)*(y1-y2))*r3*r3 - a*a))/((x1-x2)*(x1-x2) + (y1-y2)*(y1-y2))+x2; yb = (a*(y1-y2) + (x1-x2) * Math.sqrt(((x1-x2)*(x1-x2) + (y1-y2)*(y1-y2))*r3*r3 - a*a))/((x1-x2)*(x1-x2) + (y1-y2)*(y1-y2))+y2; //B,C, A, (gammax, gammay) if (Math.abs(xa - x2)+math.abs(ya - y2) <= Math.abs(xb - x2)+math.abs(yb - y2)) { gammax = xa; gammay = ya; } else { gammax = xb; gammay = yb; } //,, x z //x,z 36
r4 = Math.sqrt((0-xt)*(0-xt)+(0-zt)*(0-zt))+n; r5 = Math.sqrt((x1-xt)*(x1-xt)+(z1-zt)*(z1-zt))+n; r6 = Math.sqrt((x2-xt)*(x2-xt)+(z2-zt)*(z2-zt))+n; //z,a,b a = (Math.pow(x1,2) + Math.pow(z1,2) + Math.pow(r4,2) - Math.pow(r5,2))/2; xa = (a*x1 + z1 * Math.sqrt((x1*x1 + z1*z1)*r4*r4 - a*a))/(x1*x1 + z1*z1); za = (a*z1 - x1 * Math.sqrt((x1*x1 + z1*z1)*r4*r4 - a*a))/(x1*x1 + z1*z1); xb = (a*x1 - z1 * Math.sqrt((x1*x1 + z1*z1)*r4*r4 - a*a))/(x1*x1 + z1*z1); zb = (a*z1 + x1 * Math.sqrt((x1*x1 + z1*z1)*r4*r4 - a*a))/(x1*x1 + z1*z1); //A,B, C, (alphax, alphaz) if (Math.abs(xa - x2)+math.abs(za - x2) <= Math.abs(xb - x2)+math.abs(zb - z2)) { alphax = xa; alphaz = za; } else { alphax = xb; alphaz = zb; } //z,a,c a = (Math.pow(x2,2) + Math.pow(z2,2) + Math.pow(r4,2) - Math.pow(r6,2))/2; xa = (a*x2 + z2 * Math.sqrt((x2*x2 + z2*z2)*r4*r4 - a*a))/(x2*x2 + z2*z2); za = (a*z2 - x2 * Math.sqrt((x2*x2 + z2*z2)*r4*r4 - a*a))/(x2*x2 + z2*z2); xb = (a*x2 - z2 * Math.sqrt((x2*x2 + z2*z2)*r4*r4 - a*a))/(x2*x2 + z2*z2); zb = (a*z2 + x2 * Math.sqrt((x2*x2 + z2*z2)*r4*r4 - a*a))/(x2*x2 + z2*z2); //z,a,c, B, (betax, betaz) 37
A if (Math.abs(xa - x1)+math.abs(za - z1) <= Math.abs(xb - x1)+math.abs(zb - z1)) { betax = xa; betaz = za; } else { betax = xb; betaz = zb; } //z:c,b a = ((x1-x2)*(x1-x2) + (z1-z2)*(z1-z2) + r6*r6 - r5*r5)/2; xa = (a*(x1-x2) + (z1-z2) * Math.sqrt(((x1-x2)*(x1-x2) + (z1-z2)*(z1-z2))*r6*r6 - a*a))/((x1-x2)*(x1-x2) + (z1-z2)*(z1-z2))+x2; za = (a*(z1-z2) - (x1-x2) * Math.sqrt(((x1-x2)*(x1-x2) + (z1-z2)*(z1-z2))*r6*r6 - a*a))/((x1-x2)*(x1-x2) + (z1-z2)*(z1-z2))+z2; xb = (a*(x1-x2) - (z1-z2) * Math.sqrt(((x1-x2)*(x1-x2) + (z1-z2)*(z1-z2))*r6*r6 - a*a))/((x1-x2)*(x1-x2) + (z1-z2)*(z1-z2))+x2; zb = (a*(z1-z2) + (x1-x2) * Math.sqrt(((x1-x2)*(x1-x2) + (z1-z2)*(z1-z2))*r6*r6 - a*a))/((x1-x2)*(x1-x2) + (z1-z2)*(z1-z2))+z2; //z:b,c A (gammax, gammay) if (Math.abs(xa - x2)+math.abs(za - z2) <= Math.abs(xb - x2)+math.abs(zb - z2)) { gammax = xa; gammaz = za; } else { gammax = xb; 38
gammaz = zb; } //, r1,r2,r3 seido.seido while (Math.abs(alphax - betax) >= seidohoji) { r1 = Math.sqrt((0-xt)*(0-xt)+(0-yt)*(0-yt))+n; r2 = Math.sqrt((x1-xt)*(x1-xt)+(y1-yt)*(y1-yt))+n; r3 = Math.sqrt((x2-xt)*(x2-xt)+(y2-yt)*(y2-yt))+n; a = (Math.pow(x1,2) + Math.pow(y1,2) + Math.pow(r1,2) - Math.pow(r2,2) )/2; xa = (a*x1 + y1 * Math.sqrt((x1*x1 + y1*y1)*r1*r1 - a*a))/(x1*x1 + y1*y1); ya = (a*y1 - x1 * Math.sqrt((x1*x1 + y1*y1)*r1*r1 - a*a))/(x1*x1 + y1*y1); xb = (a*x1 - y1 * Math.sqrt((x1*x1 + y1*y1)*r1*r1 - a*a))/(x1*x1 + y1*y1); yb = (a*y1 + x1 * Math.sqrt((x1*x1 + y1*y1)*r1*r1 - a*a))/(x1*x1 + y1*y1); if (Math.abs(ya - y2) + Math.abs(xa - x2) <= Math.abs(yb - y2)+math.abs(xb - x2)){ alphax = xa; alphay = ya; } else { alphax = xb; alphay = yb; } a = (Math.pow(x2,2) + Math.pow(y2,2) + Math.pow(r1,2) - Math.pow(r3,2))/2; xa = (a*x2 + y2 * Math.sqrt((x2*x2 + y2*y2)*r1*r1 - a*a))/(x2*x2 + y2*y2); ya = (a*y2 - x2 * Math.sqrt((x2*x2 + y2*y2)*r1*r1 - a*a))/(x2*x2 + y2*y2); xb = (a*x2 - y2 * Math.sqrt((x2*x2 + y2*y2)*r1*r1 - a*a))/(x2*x2 + y2*y2); yb = (a*y2 + x2 * Math.sqrt((x2*x2 + y2*y2)*r1*r1 - a*a))/(x2*x2 + y2*y2); if (Math.abs(xa - x1)+math.abs(ya - y1) <= 39
A Math.abs(xb - x1)+math.abs(yb - y1)) { betax = xa; betay = ya; } else { betax = xb; betay = yb; } a = ((x1-x2)*(x1-x2) + (y1-y2)*(y1-y2) + r3*r3 - r2*r2)/2; xa = (a*(x1-x2) + (y1-y2) * Math.sqrt(((x1-x2)*(x1-x2) + (y1-y2)*(y1-y2))*r3*r3 - a*a))/((x1-x2)*(x1-x2) + (y1-y2)*(y1-y2))+x2; ya = (a*(y1-y2) - (x1-x2) * Math.sqrt(((x1-x2)*(x1-x2) + (y1-y2)*(y1-y2))*r3*r3 - a*a))/((x1-x2)*(x1-x2) + (y1-y2)*(y1-y2))+y2; xb = (a*(x1-x2) - (y1-y2) * Math.sqrt(((x1-x2)*(x1-x2) + (y1-y2)*(y1-y2))*r3*r3 - a*a))/((x1-x2)*(x1-x2) + (y1-y2)*(y1-y2))+x2; yb = (a*(y1-y2) + (x1-x2) * Math.sqrt(((x1-x2)*(x1-x2) + (y1-y2)*(y1-y2))*r3*r3 - a*a))/((x1-x2)*(x1-x2) + (y1-y2)*(y1-y2))+y2; if (Math.abs(xa - x2)+math.abs(ya - y2) <= Math.abs(xb - x2)+math.abs(yb - y2)) { gammax = xa; gammay = ya; } else { gammax = xb; gammay = yb; } //x,z r4 = Math.sqrt((0-xt)*(0-xt)+(0-zt)*(0-zt))+n; 40
r5 = Math.sqrt((x1-xt)*(x1-xt)+(z1-zt)*(z1-zt))+n; r6 = Math.sqrt((x2-xt)*(x2-xt)+(z2-zt)*(z2-zt))+n; //z,a,b a = (Math.pow(x1,2) + Math.pow(z1,2) + Math.pow(r4,2) - Math.pow(r5,2))/2; xa = (a*x1 + z1 * Math.sqrt((x1*x1 + z1*z1)*r4*r4 - a*a))/(x1*x1 + z1*z1); za = (a*z1 - x1 * Math.sqrt((x1*x1 + z1*z1)*r4*r4 - a*a))/(x1*x1 + z1*z1); xb = (a*x1 - z1 * Math.sqrt((x1*x1 + z1*z1)*r4*r4 - a*a))/(x1*x1 + z1*z1); zb = (a*z1 + x1 * Math.sqrt((x1*x1 + z1*z1)*r4*r4 - a*a))/(x1*x1 + z1*z1); if (Math.abs(xa - x2)+math.abs(za - x2) <= Math.abs(xb - x2)+math.abs(zb - z2)) { alphax = xa; alphaz = za; } else { alphax = xb; alphaz = zb; } a = (Math.pow(x2,2) + Math.pow(z2,2) + Math.pow(r4,2) - Math.pow(r6,2))/2; xa = (a*x2 + z2 * Math.sqrt((x2*x2 + z2*z2)*r4*r4 - a*a))/(x2*x2 + z2*z2); za = (a*z2 - x2 * Math.sqrt((x2*x2 + z2*z2)*r4*r4 - a*a))/(x2*x2 + z2*z2); xb = (a*x2 - z2 * Math.sqrt((x2*x2 + z2*z2)*r4*r4 - a*a))/(x2*x2 + z2*z2); zb = (a*z2 + x2 * Math.sqrt((x2*x2 + z2*z2)*r4*r4 - a*a))/(x2*x2 + z2*z2); //z,a,c, B, (betax, betaz) if (Math.abs(xa - x1)+math.abs(za - z1) <= Math.abs(xb - x1)+math.abs(zb - z1)) { betax = xa; 41
A betaz = za; } else { betax = xb; betaz = zb; } a = ((x1-x2)*(x1-x2) + (z1-z2)*(z1-z2) + r6*r6 - r5*r5)/2; xa = (a*(x1-x2) + (z1-z2) * Math.sqrt(((x1-x2)*(x1-x2) + (z1-z2)*(z1-z2))*r6*r6 - a*a))/((x1-x2)*(x1-x2) + (z1-z2)*(z1-z2))+x2; za = (a*(z1-z2) - (x1-x2) * Math.sqrt(((x1-x2)*(x1-x2) + (z1-z2)*(z1-z2))*r6*r6 - a*a))/((x1-x2)*(x1-x2) + (z1-z2)*(z1-z2))+z2; xb = (a*(x1-x2) - (z1-z2) * Math.sqrt(((x1-x2)*(x1-x2) + (z1-z2)*(z1-z2))*r6*r6 - a*a))/((x1-x2)*(x1-x2) + (z1-z2)*(z1-z2))+x2; zb = (a*(z1-z2) + (x1-x2) * Math.sqrt(((x1-x2)*(x1-x2) + (z1-z2)*(z1-z2))*r6*r6 - a*a))/((x1-x2)*(x1-x2) + (z1-z2)*(z1-z2))+z2; //z:b,c A (gammax, gammay) if (Math.abs(xa - x2)+math.abs(za - z2) <= Math.abs(xb - x2)+math.abs(zb - z2)) { gammax = xa; gammaz = za; } else { gammax = xb; gammaz = zb; } seido = seido + seidohoji; } // 42
System.out.println(" : " + alphax + "," + alphay + "," + alphaz); // long end = System.nanoTime(); System.out.println(" : " +(end-start)+"[ns]" ); } } 43