API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Mar 23rd, 2017
65
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 19.71 KB | None | 0 0
raw download clone embed print report
  1. import java.io.FileNotFoundException;
  2. import java.io.PrintWriter;
  3. import java.util.*;
  4.  
  5.  
  6. public class Trilateration3D {
  7. public static void main(String[] args) throws FileNotFoundException {
  8. //Redosled na vnesuvanje:
  9. //public WSN(int N, int anchorPer, double R, double L, double noise)
  10.  
  11. int N, anchorPer;
  12. double R, L, noise;
  13. Scanner s = new Scanner(System.in);
  14.  
  15.  
  16. System.out.println("N - vkupen broj na jazli, anchorPer - procent na anchor jazli");
  17. System.out.println("L - dolzina, R - radio opseg, noise - procent na sum");
  18. System.out.println("\nIzberi grafik: ");
  19. System.out.println("1: ANCHORS vs NOISE 2: NOISE vs ERROR 3: RANGE vs CONNECTIVITY");
  20.  
  21. int c = Integer.parseInt(s.nextLine());
  22. if(c == 1)
  23. {
  24. System.out.println("Vnesi po red: N - R - L - noise");
  25. String line = s.nextLine();
  26. String []parts = line.split(" ");
  27. N = Integer.parseInt(parts[0]);
  28. R = Double.parseDouble(parts[1]);
  29. L = Double.parseDouble(parts[2]);
  30. noise = Double.parseDouble(parts[3]);
  31.  
  32. //Test1(int N, double R, double L, double noise)
  33. Test1(N,R,L,noise);
  34. }
  35. else if(c == 2)
  36. {
  37. System.out.println("Vnesi po red: N - anchorPer - R - L");
  38. String line = s.nextLine();
  39. String []parts = line.split(" ");
  40. N = Integer.parseInt(parts[0]);
  41. anchorPer = Integer.parseInt(parts[1]);
  42. R = Double.parseDouble(parts[2]);
  43. L = Double.parseDouble(parts[3]);
  44.  
  45. //Test2(int N, int anchorPer,double R, double L)
  46. Test2(N, anchorPer, R, L);
  47. }
  48. else if (c == 3){
  49. System.out.println("Vnesi po red: N - anchorPer - L - noise");
  50. String line = s.nextLine();
  51. String []parts = line.split(" ");
  52. N = Integer.parseInt(parts[0]);
  53. anchorPer = Integer.parseInt(parts[1]);
  54. L = Double.parseDouble(parts[2]);
  55. noise = Double.parseDouble(parts[3]);
  56. //Test3(int N, int anchorPer, double L, double noise)
  57. Test3(N, anchorPer, L, noise);
  58.  
  59. }
  60.  
  61.  
  62. }
  63.  
  64.  
  65. public static void Test1(int N, double R, double L, double noise) throws FileNotFoundException{ //ANCHOR PERCENT vs ERROR
  66. PrintWriter pw = new PrintWriter("F:/FINKI/VI semestar/Senzorski sistemi/Trilateration/anchors.txt");
  67. /* int anchorPer = 10;
  68. double L = 300;
  69. int N = 200;
  70. double R = 60;
  71. double noise = 10;
  72. */
  73. int anchorPer = 10;
  74. while(anchorPer < 100)
  75. {
  76. double error = 0;
  77. for(int j = 0; j < 30; j++)
  78. {
  79. WSN wsn = new WSN(N, anchorPer, R, L, noise);
  80. wsn.calculate();
  81. // System.out.println(String.format("N: %d, anchorPer: %d, R: %.2f, L: %.2f, noise: %.2f", N, anchorPer,R,L,noise));
  82. double err = wsn.calculateError();
  83. error+= err;
  84.  
  85. }
  86.  
  87. error /=30;
  88. StringBuilder sb = new StringBuilder();
  89. sb.append(String.format("%d %.3f",anchorPer,error));
  90. System.out.println(String.format("ANCHORS: %d ERROR: %.3f",anchorPer,error));
  91. pw.println(sb.toString());
  92. anchorPer+=5;
  93. }
  94.  
  95. pw.close();
  96. }
  97.  
  98. public static void Test2(int N, int anchorPer,double R, double L) throws FileNotFoundException{ //NOISE vs ERROR
  99. PrintWriter pw = new PrintWriter("F:/FINKI/VI semestar/Senzorski sistemi/Trilateration/noise.txt");
  100. double noise = 10;
  101. while(noise < 100)
  102. {
  103. double error = 0;
  104. for(int j = 0; j < 30; j++)
  105. {
  106. WSN wsn = new WSN(N, anchorPer, R, L, noise);
  107. wsn.calculate();
  108. // System.out.println(String.format("N: %d, anchorPer: %d, R: %.2f, L: %.2f, noise: %.2f", N, anchorPer,R,L,noise));
  109. double err = wsn.calculateError();
  110. error+= err;
  111.  
  112.  
  113. }
  114.  
  115. error /=30;
  116. StringBuilder sb = new StringBuilder();
  117. sb.append(String.format("%.3f %.3f",noise,error));
  118. System.out.println(String.format("NOISE: %.3f ERROR: %.3f",noise,error));
  119. pw.println(sb.toString());
  120. noise+=5;
  121. }
  122.  
  123. pw.close();
  124. }
  125.  
  126.  
  127. public static void Test3(int N, int anchorPer, double L, double noise) throws FileNotFoundException{ //RANGE vs AVERAGE NUMBER OF NEARBY ANCHORS
  128. PrintWriter pw = new PrintWriter("F:/FINKI/VI semestar/Senzorski sistemi/Trilateration/range.txt");
  129. double R = 10;
  130. while(R < L)
  131. {
  132. double avg = 0;
  133. for(int j = 0; j < 30; j++)
  134. {
  135. WSN wsn = new WSN(N, anchorPer, R, L, noise);
  136. wsn.calculate();
  137. avg+=wsn.avgNeighbors();
  138.  
  139. if(R == 40)
  140. {
  141. wsn.getCoordinatesForVisualization();
  142. }
  143. }
  144.  
  145. avg/=30;
  146. StringBuilder sb = new StringBuilder();
  147. sb.append(String.format("%.3f %.3f",avg,R));
  148. System.out.println(String.format("RADIO RANGE: %.3f AVERAGE NUMBER OF NEARBY ANCHORS: %.3f",R, avg));
  149. pw.println(sb.toString());
  150. R+=10;
  151. }
  152.  
  153. pw.close();
  154. }
  155.  
  156.  
  157.  
  158. }
  159.  
  160.  
  161. class Node{
  162. double X,Y,Z;
  163. boolean anchor;
  164. boolean original;
  165. double Xs,Ys,Zs;
  166. List<Node> nearbyAnchors; //anchors vo radio opseg na jazolot
  167. List<Double> distanceFromAnchors;
  168.  
  169.  
  170. List<Node> points;
  171. List<Node> innerPoints;
  172.  
  173. public Node(double X, double Y, double Z,boolean anchor){
  174. this.X = X;
  175. this.Y = Y;
  176. this.Z = Z;
  177. this.anchor = anchor;
  178.  
  179. if(anchor)
  180. {
  181. original = true;
  182. Xs = X;
  183. Ys = Y;
  184. Zs = Z;
  185. }
  186. else {
  187. original = false;
  188. Xs = 0;
  189. Ys = 0;
  190. Zs = 0;
  191. }
  192.  
  193. nearbyAnchors = new ArrayList<Node>();
  194. points = new ArrayList<Node>();
  195. innerPoints = new ArrayList<Node>();
  196. }
  197.  
  198.  
  199. public boolean calculateCoordinates(boolean noiseInd, double noise){ //Presmetka na Xs i Ys, noiseInd = true ako ima noise
  200. distanceFromAnchors = new ArrayList<Double>();
  201. points = new ArrayList<Node>();
  202. innerPoints = new ArrayList<Node>();
  203.  
  204. Random rand = new Random();
  205. //[dist - r%*dist, dist + r%*dist]
  206.  
  207. for(int i = 0; i < nearbyAnchors.size(); i++)
  208. {
  209. double Xa = nearbyAnchors.get(i).X;
  210. double Ya = nearbyAnchors.get(i).Y;
  211. double Za = nearbyAnchors.get(i).Z;
  212.  
  213. double dist = distance(X,Y,Z,Xa, Ya,Za);
  214. if(noiseInd) //Ako ima noise
  215. {
  216. double n = noise * rand.nextDouble();
  217. double s = -1;
  218. if(rand.nextBoolean())
  219. s = 1;
  220. n*=s;
  221.  
  222. dist += (dist/100.0)*n;
  223. }
  224. distanceFromAnchors.add(dist);
  225. }
  226.  
  227. double radius1 = Double.MAX_VALUE; //prvo najmalo rastojanie
  228. double radius2 = Double.MAX_VALUE; // vtoro najmalo rastojanie
  229. double radius3 = Double.MAX_VALUE; //treto najmalo rastojanie
  230.  
  231. int j1 = -1, j2 = -1, j3 = -1; //Indeksi na trite anchors
  232.  
  233.  
  234. for(int i = 0; i < distanceFromAnchors.size();i++){
  235. if(distanceFromAnchors.get(i) < radius1)
  236. {
  237. radius3 = radius2;
  238. radius2 = radius1;
  239. radius1 = distanceFromAnchors.get(i);
  240. j3 = j2;
  241. j2 = j1;
  242. j1 = i;
  243. }
  244. else if(distanceFromAnchors.get(i) < radius2)
  245. {
  246. radius3 = radius2;
  247. radius2 = distanceFromAnchors.get(i);
  248. j3 = j2;
  249. j2 = i;
  250. }
  251. else if(distanceFromAnchors.get(i) < radius3){
  252. radius3 = distanceFromAnchors.get(i);
  253. j3 = i;
  254. }
  255. }
  256.  
  257. return true;
  258.  
  259. }
  260.  
  261. public void trilateration(Node p1, Node p2, Node p3){
  262.  
  263. }
  264.  
  265. public double distance(double x1, double y1, double z1, double x2, double y2, double z2){ //Evklidovo rastojanie
  266. return Math.sqrt((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1)+(z2-z1)*(z2-z1));
  267. }
  268.  
  269.  
  270.  
  271. public double sqr(double a) { //kvadrat
  272. return a * a;
  273. }
  274.  
  275. public double norm(Node a) {
  276. return Math.sqrt(sqr(a.X) + sqr(a.Y) + sqr(a.Z));
  277. }
  278.  
  279. public double dot(Node a, Node b) {
  280. return a.X * b.X + a.Y * b.Y + a.Z * b.Z;
  281. }
  282.  
  283. public Node vector_subtract(Node a, Node b) {
  284. Node node = new Node(a.X-b.X, a.Y-b.Y, a.Z-b.Z, false);
  285. return node;
  286. }
  287.  
  288. public Node vector_add(Node a, Node b) {
  289. Node node = new Node(a.X+b.X, a.Y+b.Y, a.Z+b.Z, false);
  290. return node;
  291. }
  292.  
  293. public Node vector_divide(Node a, double b) {
  294. Node node = new Node(a.X/b, a.Y/b, a.Z/b,false);
  295. return node;
  296. }
  297.  
  298. public Node vector_multiply(Node a, double b) {
  299. Node node = new Node(a.X*b, a.Y*b, a.Z*b,false);
  300. return node;
  301. }
  302.  
  303. public Node vector_cross(Node a, Node b) {
  304. Node node = new Node(a.Y * b.Z - a.Z * b.Y,a.Z * b.X - a.X * b.Z,a.X * b.Y - a.Y * b.X,false);
  305. return node;
  306. }
  307.  
  308.  
  309.  
  310. public void allIntersectingPoints(Node n1, Node n2, Node n3){ //Za site presecni tocki na tri kruznici so centri vo n1, n2, n3
  311. List<Circle> circles = new ArrayList<Circle>();
  312. circles.add(new Circle(n1, distance(n1.X, n1.Y, n1.Z, n2.X, n2.Y,n2.Z)));
  313. circles.add(new Circle(n2, distance(n2.X, n2.Y, n2.Z, n3.X, n3.Y,n3.Z)));
  314. circles.add(new Circle(n3, distance(n3.X, n3.Y, n3.Z, n1.X, n1.Y,n1.Z)));
  315.  
  316. intersectingPoints(circles.get(0), circles.get(1));
  317. intersectingPoints(circles.get(1), circles.get(2));
  318. intersectingPoints(circles.get(2), circles.get(0));
  319.  
  320.  
  321. for(int i = 0; i < points.size(); i++)
  322. {
  323. if(isContainedInCircles(points.get(i), circles))
  324. innerPoints.add(points.get(i));
  325. }
  326.  
  327.  
  328. }
  329.  
  330. public void intersectingPoints(Circle c1, Circle c2){ // Naogja presecni tocki na dve kruznici
  331.  
  332. Node p1 = c1.center;
  333. Node p2 = c2.center;
  334. double r1 = c1.radius;
  335. double r2 = c2.radius;
  336.  
  337. double dist = distance(p1.X, p1.Y, p1.Z,p2.X, p2.Y,p2.Z);
  338. if(dist >= (r1+r2) || dist <= Math.abs(r1-r2))
  339. return;
  340.  
  341. double a = (r1*r1-r2*r2 + dist*dist)/(2*dist);
  342. double h = Math.sqrt(r1*r1 - a*a);
  343.  
  344. double x0 = p1.X + a*(p2.X - p1.X)/dist;
  345. double y0 = p1.Y + a*(p2.Y - p1.Y)/dist;
  346. double rx = -(p2.Y - p1.Y) * (h/dist);
  347. double ry = -(p2.X - p1.X) * (h / dist);
  348.  
  349. Node n1 = new Node(x0+rx,y0-ry,false);
  350. Node n2 = new Node(x0-rx,y0+ry,false);
  351. points.add(n1);
  352. points.add(n2);
  353. }
  354.  
  355.  
  356. public boolean getCoordinates(){ //Presmetka na koordinati na teziste
  357.  
  358. if(innerPoints.size() == 0)
  359. return false;
  360.  
  361. double Xn = 0, Yn = 0;
  362. for(int i = 0; i < innerPoints.size(); i++)
  363. {
  364. Xn+=innerPoints.get(i).X;
  365. Yn+=innerPoints.get(i).Y;
  366. }
  367.  
  368. Xn/=innerPoints.size();
  369. Yn/=innerPoints.size();
  370.  
  371. Xs = Xn;
  372. Ys = Yn;
  373.  
  374. return true;
  375. }
  376.  
  377. public boolean isContainedInCircles(Node node, List<Circle> circles){ //Dali tockata pripagja na site kruznici
  378. for(int i = 0; i < circles.size(); i++)
  379. {
  380. if(distance(node.X, node.Y,node.Z,circles.get(i).center.X,circles.get(i).center.Y,circles.get(i).center.Z) > circles.get(i).radius)
  381. return false;
  382. }
  383.  
  384. return true;
  385. }
  386.  
  387.  
  388. public String printCoordinates(){
  389. if(!anchor)
  390. return String.format("X: %-8.02f Y: %-8.02f Xs i Ys ne moze da se presmetaat\n", X,Y);
  391. else if(original)
  392. return String.format("X: %-8.02f Y: %-8.02f ---> Original anchor", X,Y);
  393. else
  394. return String.format("X: %-8.02f Y: %-8.02f Xs: %-8.02f Ys: %-8.02f\n", X,Y,Xs,Ys);
  395. }
  396.  
  397. public String toString(){
  398. if(original)
  399. return String.format("X: %-8.02f Y: %-8.02f -> Original anchor\n", X,Y);
  400. else
  401. return String.format("X: %-8.2f Y: %.2f\n", X,Y);
  402. }
  403. }
  404.  
  405. class WSN{
  406. List<Node> nodes; //lista od site jazli
  407. List<Node> anchors; //lista od anchors
  408. int N, anchorPer; //N: vkupen broj na jazli, anchorPer: procent na anchor jazli
  409. double R, L, noise, anchorNum; //L: dolzina na oblast, R: radio opseg, noise: sum na signal, anchorNum: broj na anchor jazli sto treba da se generiraat
  410. boolean anchor;
  411.  
  412. public WSN(int N, int anchorPer, double R, double L, double noise) throws FileNotFoundException{
  413. nodes = new ArrayList<Node>();
  414. anchors = new ArrayList<Node>();
  415. this.N = N;
  416. this.L = L;
  417. this.R = R;
  418. this.noise = noise;
  419. this.anchorPer = anchorPer;
  420. anchor = false;
  421.  
  422. Random rand = new Random();
  423. PrintWriter pw = new PrintWriter("F:/FINKI/VI semestar/Senzorski sistemi/Trilateration/anchorsCoordinates.txt");
  424. PrintWriter pw2 = new PrintWriter("F:/FINKI/VI semestar/Senzorski sistemi/Trilateration/nodesCoordinates.txt");
  425.  
  426. int counter = 0; //counter za anchor jazli
  427. anchorNum = ((double)N/100)*anchorPer; //broj na anchor jazli sto treba da se generiraat
  428.  
  429. for(int i = 0; i < N; i++){
  430. double x = L * rand.nextDouble();
  431. double y = L * rand.nextDouble();
  432. double z = L * rand.nextDouble();
  433.  
  434. if(counter < anchorNum){
  435. anchor = true;
  436. counter++;
  437. }
  438. else
  439. anchor = false;
  440.  
  441. Node node = new Node(x, y, z,anchor); //Generiranje na nov jazol i dodavanje vo listata
  442. nodes.add(node);
  443.  
  444. if(anchor)
  445. {
  446. anchors.add(node);
  447. pw.append(String.format("%.2f %.2f\n", node.X,node.Y));
  448. }
  449. else
  450. {
  451. pw2.append(String.format("%.2f %.2f\n", node.X,node.Y));
  452. }
  453. }
  454.  
  455. pw.close();
  456. pw2.close();
  457. }
  458.  
  459. public double distance(double x1, double y1, double z1,double x2, double y2, double z2){ //Evklidovo rastojanie
  460. return Math.sqrt((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1)+(z2-z1)*(z2-z1));
  461. }
  462.  
  463.  
  464. public void calculate(){
  465. boolean noiseInd = false; // (dali ima noise ili ne)
  466.  
  467. if(noise != 0)
  468. noiseInd = true;
  469.  
  470. this.addAnchors(); //Na pocetok, vo listite na sekoj jazol se dodavaat prvicnite anchor jazli (ako se vo radio opseg)
  471.  
  472. int counter = 0;
  473. int prevCounter = -1;
  474.  
  475. while(true){
  476.  
  477. for(int i = 0; i < nodes.size();i++){
  478. if(!nodes.get(i).anchor && nodes.get(i).nearbyAnchors.size() >= 3){
  479. double tmp1 = nodes.get(i).Xs;
  480. double tmp2 = nodes.get(i).Ys;
  481. nodes.get(i).calculateCoordinates(noiseInd, noise);
  482. nodes.get(i).anchor = true; //Stom kje se presmetaat Xs i Ys, jazolot stanuva anchor
  483.  
  484. if(tmp1!=nodes.get(i).Xs || tmp2!=nodes.get(i).Ys) //Ako ima nova presmetana vrednost za Xs ili Ys
  485. counter++;
  486.  
  487. for(int j = 0; j < nodes.size(); j++)
  488. {
  489. if(j != i && !nodes.get(j).anchor){
  490. double dist = this.distance(nodes.get(i).X, nodes.get(i).Y, nodes.get(i).Z,nodes.get(j).X, nodes.get(j).Y,nodes.get(j).Z);
  491. if(dist<=R)
  492. nodes.get(j).nearbyAnchors.add(nodes.get(i)); //Noviot anchor se dodava kaj drugite jazli (ako e vo nivniot radio opseg)
  493. }
  494. }
  495. }
  496. }
  497.  
  498. if(counter == prevCounter)
  499. break;
  500. else
  501. {
  502. prevCounter = counter;
  503. }
  504. }
  505. }
  506.  
  507. public void addAnchors(){
  508. //Za sekoj jazol sto ne e anchor se naogjaat anchor jazlite sto se vo radio opsegot
  509. for(int i = 0; i < nodes.size(); i++)
  510. {
  511. if(!nodes.get(i).original)
  512. {
  513. for(int j = 0; j < anchors.size(); j++)
  514. {
  515. double dist = this.distance(nodes.get(i).X, nodes.get(i).Y, nodes.get(i).Z,anchors.get(j).X, anchors.get(j).Y,nodes.get(i).Z);
  516. if(dist<=R)
  517. nodes.get(i).nearbyAnchors.add(anchors.get(j));
  518. }
  519. }
  520. }
  521. }
  522.  
  523.  
  524. public double avgNeighbors(){
  525. double avg = 0;
  526.  
  527. for(int i = 0; i < nodes.size(); i++)
  528. {
  529. avg+=nodes.get(i).nearbyAnchors.size();
  530. }
  531.  
  532. avg/=nodes.size();
  533.  
  534. return avg;
  535. }
  536.  
  537. public double calculateError(){ //Presmetka na greska
  538. double error = 0;
  539.  
  540. for(int i = 0; i < nodes.size(); i++)
  541. {
  542. if(nodes.get(i).anchor && !nodes.get(i).original)
  543. {
  544. double err = distance(nodes.get(i).X, nodes.get(i).Y,nodes.get(i).Z, nodes.get(i).Xs, nodes.get(i).Ys,nodes.get(i).Zs);
  545. error+=err;
  546. }
  547. }
  548.  
  549. //error/=(N-anchorNum)*1.0;
  550. error /= N*1.0;
  551.  
  552. //System.out.println(String.format("ERROR: %.3f", error));
  553.  
  554. double error_per = (error/R)*100.0;
  555. //System.out.println(String.format("ERROR PERCENTAGE: %.3f", error_per));
  556.  
  557. return error_per;
  558. }
  559.  
  560. public String toString(){
  561. StringBuilder sb = new StringBuilder();
  562. sb.append("Wireless Sensor Network\n");
  563.  
  564. for(int i = 0; i < nodes.size();i++)
  565. {
  566. sb.append(nodes.get(i).toString());
  567. }
  568. return sb.toString();
  569. }
  570.  
  571. public void printCoordinates(){
  572. for(int i =0; i < nodes.size();i++)
  573. System.out.println(nodes.get(i).printCoordinates());
  574. }
  575.  
  576. public void printDistance(){
  577. for(int i = 0; i < nodes.size(); i++)
  578. System.out.println(distance(nodes.get(i).X, nodes.get(i).Y, nodes.get(i).Z, nodes.get(i).Xs, nodes.get(i).Ys,nodes.get(i).Zs));
  579. }
  580.  
  581.  
  582. public void getCoordinatesForVisualization() throws FileNotFoundException{ //Gi zapisuva vo txt za crtanje
  583. PrintWriter pw1 = new PrintWriter("F:/FINKI/VI semestar/Senzorski sistemi/Trilateration/anchorsCoordinatesFinal.txt");
  584. PrintWriter pw2 = new PrintWriter("F:/FINKI/VI semestar/Senzorski sistemi/Trilateration/nodesCoordinatesFinal.txt");
  585. PrintWriter pw3 = new PrintWriter("F:/FINKI/VI semestar/Senzorski sistemi/Trilateration/unlocatedNodesCoordinatesFinal.txt");
  586.  
  587. for(int i = 0; i < nodes.size();i++)
  588. {
  589. if(nodes.get(i).original)
  590. pw1.append(String.format("%.2f %.2f\n", nodes.get(i).X,nodes.get(i).Y));
  591. else if (nodes.get(i).anchor)
  592. pw2.append(String.format("%.2f %.2f\n", nodes.get(i).X,nodes.get(i).Y));
  593. else
  594. pw3.append(String.format("%.2f %.2f\n", nodes.get(i).X,nodes.get(i).Y));
  595. }
  596.  
  597. pw1.close();
  598. pw2.close();
  599. pw3.close();
  600. }
  601. }
  602.  
  603. class Circle{
  604. Node center;
  605. double radius;
  606.  
  607. public Circle(Node center, double radius){
  608. this.center = center;
  609. this.radius = radius;
  610. }
  611. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!