├── .gitignore ├── LICENSE ├── README.md ├── cover.png ├── mCPP-optimized-DARP.iml ├── out └── artifacts │ └── mCPP_optimized_DARP_jar │ └── mCPP-optimized-DARP.jar ├── pom.xml └── src ├── main ├── java │ ├── Main.java │ └── pathPlanning │ │ ├── darp │ │ ├── CalculateTrajectories.java │ │ ├── ConnectComponent.java │ │ ├── DARP.java │ │ ├── DARPinPoly.java │ │ ├── Edge.java │ │ └── Kruskal.java │ │ ├── handleGeo │ │ ├── ConvCoords.java │ │ ├── Dist.java │ │ ├── InPolygon.java │ │ ├── NodesInPoly.java │ │ └── coordinates │ │ │ ├── ECEF.java │ │ │ ├── NED.java │ │ │ └── WGS84.java │ │ ├── helpers │ │ └── MinMax.java │ │ └── nodesPlacementOptimization │ │ ├── Rotate.java │ │ ├── SimulatedAnnealing.java │ │ └── Transformations.java └── resources │ ├── META-INF │ └── MANIFEST.MF │ ├── inputVariables.json │ ├── inputVariables1.json │ ├── inputVariables_indicative.json │ ├── map_pol.html │ └── visualizeWGS84.m └── test └── java ├── DARPTester.java └── Poly2Waypoints.java /.gitignore: -------------------------------------------------------------------------------- 1 | # Covers JetBrains IDEs: IntelliJ, RubyMine, PhpStorm, AppCode, PyCharm, CLion, Android Studio, WebStorm and Rider 2 | # Reference: https://intellij-support.jetbrains.com/hc/en-us/articles/206544839 3 | 4 | # User-specific stuff 5 | .idea/ 6 | .idea/**/workspace.xml 7 | .idea/**/tasks.xml 8 | .idea/**/usage.statistics.xml 9 | .idea/**/dictionaries 10 | .idea/**/shelf 11 | 12 | # Generated files 13 | .idea/**/contentModel.xml 14 | 15 | # Sensitive or high-churn files 16 | .idea/**/dataSources/ 17 | .idea/**/dataSources.ids 18 | .idea/**/dataSources.local.xml 19 | .idea/**/sqlDataSources.xml 20 | .idea/**/dynamic.xml 21 | .idea/**/uiDesigner.xml 22 | .idea/**/dbnavigator.xml 23 | 24 | target/ 25 | 26 | # Gradle 27 | .idea/**/gradle.xml 28 | .idea/**/libraries 29 | 30 | # Gradle and Maven with auto-import 31 | # When using Gradle or Maven with auto-import, you should exclude module files, 32 | # since they will be recreated, and may cause churn. Uncomment if using 33 | # auto-import. 34 | # .idea/artifacts 35 | # .idea/compiler.xml 36 | # .idea/jarRepositories.xml 37 | # .idea/modules.xml 38 | # .idea/*.iml 39 | # .idea/modules 40 | # *.iml 41 | # *.ipr 42 | 43 | # CMake 44 | cmake-build-*/ 45 | 46 | # Mongo Explorer plugin 47 | .idea/**/mongoSettings.xml 48 | 49 | # File-based project format 50 | *.iws 51 | 52 | # mpeltonen/sbt-idea plugin 53 | .idea_modules/ 54 | 55 | # JIRA plugin 56 | atlassian-ide-plugin.xml 57 | 58 | # Cursive Clojure plugin 59 | .idea/replstate.xml 60 | 61 | # Crashlytics plugin (for Android Studio and IntelliJ) 62 | com_crashlytics_export_strings.xml 63 | crashlytics.properties 64 | crashlytics-build.properties 65 | fabric.properties 66 | 67 | # Editor-based Rest Client 68 | .idea/httpRequests 69 | 70 | # Android studio 3.1+ serialized cache file 71 | .idea/caches/build_file_checksums.ser -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 savvas-ap 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Cooperative Multi-UAV Coverage Mission Planning Platform for Remote Sensing Applications - Path Planning back-end 2 | 3 | ## Description 4 | The project in this repository is a multi-robot coverage path planning (mCPP) module, utilizing [STC](https://link.springer.com/article/10.1023/A:1016610507833) 5 | and [DARP](https://github.com/athakapo/DARP). This work has been presented, evaluated and tested in the [Autonomous Robots](https://link.springer.com/article/10.1007%2Fs10514-021-10028-3) journal. 6 | The implemented algorithm is optimized in order to efficiently cope with real-life multi-UAV coverage missions, utilizing a novel 7 | optimization scheme based on [simulated annealing](https://www.researchgate.net/publication/6026283_Optimization_by_Simulated_Annealing) algorithm. 8 | The overall methodology achieves state-of-the-art performance in mCPP problem (see comparison with simulated evaluation [here](https://github.com/savvas-ap/cpp-simulated-evaluations)). 9 | 10 | An on-line instance of an end-to-end mission planner utilizing this mCPP module can be found here: https://choosepath.ddns.net/ 11 | 12 | In addition, a demonstrative video of the platform can be found [here](https://www.youtube.com/watch?v=JQrqt1dS4A8), including examples of missions' 13 | creation, management and execution, real-life operations, and indicative results that can be acquired. 14 | 15 | ![Mission examples](cover.png) 16 | 17 | The figure above, shows four examples of missions generated by this mCPP algorithm and visualized through the mission 18 | planning platform mentioned above. 19 | 20 | ### Highlights: 21 | - Support of multiple robots/vehicles 22 | - Support of convex and non-convex, very complex-shaped polygon regions 23 | - Support of multiple obstacles inside the operational area 24 | - Fair and proportional area allocation of the overall region to each robot/vehicle 25 | - Utilization of the initial positions of the vehicles for both the area allocation and the path planning, in order to make 26 | the methodology more efficient 27 | - Energy aware features (paths' length reduction, turns reduction, avoidance of redundant movements that do not contribute in the coverage, etc.) 28 | 29 | 30 | ## Input/Output: 31 | The algorithm receives as input the following: 32 | - The number of robots/vehicles 33 | - The desired scanning density (scanning density corresponds to the desired distance between two sequential trajectories in meters) 34 | - A polygon Region of Interest (ROI), formatted in WGS84 coordinate system 35 | - A set of obstacles (polygons formatted in WGS84 coordinate system) inside the ROI (optional) 36 | - A boolean variable named pathsStrictlyInPoly, to select mode between (paths strictly in poly/better coverage) 37 | - The initial positions of the vehicles (optional - if not provided, random will be used instead | Note that the number 38 | of the initial positions should always be the same as the number of robots/vehicles) 39 | - The desired percentages for proportional area allocation (optional - if not provided, equal will be used instead | Note 40 | that the number of the percentages should always be the same as the number of robots/vehicles and their sum should be 1) 41 | 42 | As an output, the algorithm provides set of waypoints (path), for each vehicle involved in the mission, in order 43 | to cooperatively completely cover the ROI. 44 | 45 | ## Run the project: 46 | In the src/main/resources folder is included a JSON file, containing input parameters for an example mission with 3 vehicles. 47 | The input variables are included in the JSON with the same order as described above. 48 | 49 | ### 1st way: run the jar file 50 | 51 | 52 | In the out/artifacts/mCPP_optimized_DARP_jar you can find the project packed in a jar file. The jar expects as an input the path 53 | for a JSON file as the one included in the resources. To run the jar from its current location with the json in the resources folder 54 | run: 55 | 56 | ```bash 57 | java -jar mCPP-optimized-DARP.jar "../../../src/main/resources/inputVariables.json" 58 | ``` 59 | 60 | ### 2nd way: run the Main.java 61 | 62 | The Main class of the project expects as argument the path for such a JSON file as well. You can find the JSON file needed to run the main 63 | in the resources folder, as shown below: 64 | 65 | ``` 66 | "src/main/resources/inputVariables.json" 67 | ``` 68 | 69 | ### 3rd way: run the Poly2Waypoints.java 70 | In the src/test.java folder you can find the Poly2Waypoints class. In this class you can find the definition of some example 71 | input variables to run the project, with the same order as described above. 72 | 73 | ### Additional tools 74 | A simple map tool where you can create polygons over a map and copy the coordinates, and a simple matlab code to visualize 75 | the output of this project are also included in the "src/main/resources/" folder. 76 | 77 | ## Relevant/useful material: 78 | Platform's demonstrative [video](https://www.youtube.com/watch?v=JQrqt1dS4A8) 79 | 80 | Platform's on-line hosted [instance](http://choosepath.ddns.net) 81 | 82 | Article's [page](https://link.springer.com/article/10.1007%2Fs10514-021-10028-3) 83 | 84 | arXiv [page](https://arxiv.org/abs/2201.07030) 85 | 86 | Paper's results [repo](https://github.com/savvas-ap/cpp-simulated-evaluations) 87 | 88 | DARP original Java implementation [repo](https://github.com/athakapo/DARP) 89 | 90 | DARP recent Python implementation [repo](https://github.com/alice-st/DARP) 91 | 92 | DARP [paper](https://kapoutsis.info/wp-content/uploads/2017/02/j3.pdf) 93 | 94 | DARP [Medium article](https://medium.com/@athanasios.kapoutsis/darp-divide-areas-algorithm-for-optimal-multi-robot-coverage-path-planning-2fed77b990a3) 95 | 96 | ## Cite as: 97 | 98 | ``` 99 | @article{Apostolidis_2022, 100 | doi = {10.1007/s10514-021-10028-3}, 101 | url = {https://doi.org/10.1007%2Fs10514-021-10028-3}, 102 | year = 2022, 103 | month = {jan}, 104 | publisher = {Springer Science and Business Media {LLC}}, 105 | author = {Savvas D. Apostolidis and Pavlos Ch. Kapoutsis and Athanasios Ch. Kapoutsis and Elias B. Kosmatopoulos}, 106 | title = {Cooperative multi-{UAV} coverage mission planning platform for remote sensing applications}, 107 | journal = {Autonomous Robots} 108 | } 109 | ``` 110 | 111 | 112 | -------------------------------------------------------------------------------- /cover.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/savvas-ap/mCPP-optimized-DARP/bf0fa3701f98e302ebd5c4cc732decc4e9e92328/cover.png -------------------------------------------------------------------------------- /mCPP-optimized-DARP.iml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /out/artifacts/mCPP_optimized_DARP_jar/mCPP-optimized-DARP.jar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/savvas-ap/mCPP-optimized-DARP/bf0fa3701f98e302ebd5c4cc732decc4e9e92328/out/artifacts/mCPP_optimized_DARP_jar/mCPP-optimized-DARP.jar -------------------------------------------------------------------------------- /pom.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 4.0.0 6 | 7 | org.example 8 | mCPP-optimized-DARP 9 | 1.0-SNAPSHOT 10 | 11 | 12 | com.googlecode.json-simple 13 | json-simple 14 | 1.1.1 15 | 16 | 17 | 18 | 19 | 8 20 | 8 21 | 22 | 23 | -------------------------------------------------------------------------------- /src/main/java/Main.java: -------------------------------------------------------------------------------- 1 | import org.json.simple.JSONArray; 2 | import org.json.simple.JSONObject; 3 | import org.json.simple.parser.JSONParser; 4 | import pathPlanning.darp.DARPinPoly; 5 | 6 | import java.io.FileReader; 7 | import java.util.ArrayList; 8 | 9 | public class Main { 10 | 11 | private static int droneNo; 12 | private static int scanningDensity; 13 | private static double[][] polygon; 14 | private static double[][][] obstacles; 15 | private static boolean pathsStrictlyInPoly; 16 | private static double[][] initialPos; 17 | private static double[] rPortions; 18 | 19 | private static String pathJSON; 20 | private static boolean pathDefined = false; 21 | 22 | public static void main (String[] args) { 23 | 24 | //-------------------------------------------------------------------------------------------------------------- 25 | // Parse JSON with mission parameters 26 | //-------------------------------------------------------------------------------------------------------------- 27 | try { 28 | pathJSON = args[0]; 29 | pathDefined = true; 30 | } 31 | catch (ArrayIndexOutOfBoundsException e){ 32 | System.out.println("No path for JSON file defined"); 33 | } 34 | 35 | if (pathDefined) { // provide input variables with .txt file in resources 36 | JSONParser parser = new JSONParser(); 37 | try { 38 | Object obj = parser.parse(new FileReader(pathJSON)); 39 | 40 | // A JSON object. Key value pairs are unordered. JSONObject supports java.util.Map interface. 41 | JSONObject jsonObject = (JSONObject) obj; 42 | 43 | droneNo = ((Long) jsonObject.get("droneNo")).intValue(); 44 | 45 | scanningDensity = ((Long) jsonObject.get("scanningDensity")).intValue(); 46 | 47 | JSONArray temp = (JSONArray) jsonObject.get("polygon"); 48 | polygon = new double[temp.size()][2]; 49 | JSONObject vertex; 50 | for (int i=0; i0) { 59 | obstacles = new double[obst.size()][][]; 60 | for (int i=0; i0){ 79 | initialPos = new double[temp.size()][2]; 80 | for (int i=0; i0){ 91 | rPortions = new double[temp.size()]; 92 | for (int i=0; i< temp.size(); i++){ 93 | rPortions[i] = (double) temp.get(i); 94 | } 95 | }else{ 96 | rPortions = new double[]{}; 97 | } 98 | 99 | } catch (Exception e) { 100 | e.printStackTrace(); 101 | } 102 | } 103 | //-------------------------------------------------------------------------------------------------------------- 104 | 105 | 106 | 107 | //-------------------------------------------------------------------------------------------------------------- 108 | // Run optimized DARP mCPP algorithm 109 | //-------------------------------------------------------------------------------------------------------------- 110 | DARPinPoly mission = new DARPinPoly(); 111 | if(initialPos.length > 0){ 112 | if (rPortions.length > 0){ 113 | mission.waypointsDARP(droneNo, scanningDensity, polygon, obstacles, pathsStrictlyInPoly, initialPos, rPortions); 114 | } else { 115 | mission.waypointsDARP(droneNo, scanningDensity, polygon, obstacles, pathsStrictlyInPoly, initialPos); 116 | } 117 | } else { 118 | if (rPortions.length > 0){ 119 | mission.waypointsDARP(droneNo, scanningDensity, polygon, obstacles, pathsStrictlyInPoly, rPortions); 120 | } else { 121 | mission.waypointsDARP(droneNo, scanningDensity, polygon, obstacles, pathsStrictlyInPoly); 122 | } 123 | } 124 | mission.printWaypointsWGS84(); 125 | 126 | ArrayList> missionWaypoints = mission.getMissionWaypointsWGS84(); 127 | //-------------------------------------------------------------------------------------------------------------- 128 | 129 | } 130 | 131 | } 132 | -------------------------------------------------------------------------------- /src/main/java/pathPlanning/darp/CalculateTrajectories.java: -------------------------------------------------------------------------------- 1 | package pathPlanning.darp; 2 | 3 | import java.util.ArrayList; 4 | import java.util.HashSet; 5 | import java.util.TreeSet; 6 | import java.util.Vector; 7 | 8 | /** 9 | * Created by atkap on 5/24/2016. 10 | */ 11 | 12 | public class CalculateTrajectories { 13 | 14 | private TreeSet[] nodes; // Array of connected components 15 | private int MAX_NODES; 16 | private int rows, cols; 17 | private Vector MSTvector; 18 | private int MSTedges; 19 | private HashSet AllEdges; 20 | private ArrayList PathSequence; 21 | 22 | 23 | CalculateTrajectories(int r, int c, Vector MST) { 24 | // Constructor 25 | this.MAX_NODES = 4*r*c; 26 | this.PathSequence = new ArrayList<>(); 27 | this.rows=r; 28 | this.cols=c; 29 | this. MSTvector = MST; 30 | this.MSTedges = MSTvector.size(); 31 | this.AllEdges = new HashSet<>(); 32 | this.nodes = new TreeSet[MAX_NODES]; // Create array for components 33 | } 34 | 35 | 36 | public void initializeGraph(boolean [][] A, boolean connect4) { 37 | 38 | for (int i=0;i<2*rows;i++){ 39 | for (int j=0;j<2*cols;j++){ 40 | if (A[i][j]){ 41 | 42 | if (i>0 && A[i-1][j]) {AddToAllEdges(i*2*cols+j, (i-1)*2*cols+j, 1);} 43 | if (i<2*rows-1 && A[i+1][j]) {AddToAllEdges(i*2*cols+j, (i+1)*2*cols+j, 1);} 44 | if (j>0 && A[i][j-1]) {AddToAllEdges(i*2*cols+j, i*2*cols+j-1, 1);} 45 | if (j<2*cols-1 && A[i][j+1]) {AddToAllEdges(i*2*cols+j, i*2*cols+j+1, 1);} 46 | 47 | if (!connect4){ 48 | if (i>0 && j>0 && A[i-1][j-1]) {AddToAllEdges(i*2*cols+j, (i-1)*2*cols+j-1, 1);} 49 | if (i<2*rows-1 && j<2*cols-1 && A[i+1][j+1]) {AddToAllEdges(i*2*cols+j, (i+1)*2*cols+j+1, 1);} 50 | if (i>2*rows-1 && j>0 && A[i+1][j-1]) {AddToAllEdges(i*2*cols+j, (i+1)*2*cols+j-1, 1);} 51 | if (i>0 && j<2*cols-1 && A[i-1][j+1]) {AddToAllEdges(i*2*cols+j, (i-1)*2*cols+j+1, 1);} 52 | } 53 | } 54 | } 55 | } 56 | 57 | } 58 | 59 | private void AddToAllEdges(int from, int to, int cost){ 60 | //allEdges.add(new Edge(from, to, cost)); // Update priority queue 61 | AllEdges.add(new Edge(from, to, cost)); // Update priority queue 62 | if (nodes[from] == null) { 63 | // Create set of connect components [singleton] for this node 64 | nodes[from] = new TreeSet(); 65 | //nodes[from].add(new Integer(from)); 66 | } 67 | nodes[from].add(new Integer(to)); 68 | 69 | if (nodes[to] == null) { 70 | // Create set of connect components [singleton] for this node 71 | nodes[to] = new TreeSet(); 72 | //nodes[to].add(new Integer(to)); 73 | } 74 | nodes[to].add(new Integer(from)); 75 | } 76 | 77 | 78 | public void RemoveTheAppropriateEdges(){ 79 | 80 | int alpha,maxN,minN; 81 | Edge eToRemove,eToRemoveMirr,eToRemove2,eToRemove2Mirr; 82 | for (int i=0;i RemovedNodes = new HashSet<>(); 137 | int prevNode,i,j,previ,prevj,offset; 138 | 139 | ArrayList movement = new ArrayList<>(); 140 | movement.add(2*cols); 141 | movement.add(-1); 142 | movement.add(-2*cols); 143 | movement.add(1); 144 | 145 | boolean found=false; 146 | prevNode = 0; 147 | for (int idx=0;idx<4;idx++) { 148 | if (nodes[currentNode].contains(currentNode + movement.get(idx))){ 149 | prevNode = currentNode + movement.get(idx); 150 | found = true; 151 | break; 152 | } 153 | } 154 | if (!found) {return;} 155 | 156 | do { 157 | if (currentNode != StartingNode) 158 | RemovedNodes.add(currentNode); 159 | 160 | offset = movement.indexOf(prevNode-currentNode); 161 | 162 | prevNode = currentNode; 163 | 164 | found = false; 165 | for (int idx=0;idx<4;idx++){ 166 | if (nodes[prevNode].contains(prevNode+movement.get((idx+offset)%4)) && 167 | !RemovedNodes.contains(prevNode+movement.get((idx+offset)%4))) { 168 | currentNode = prevNode + movement.get((idx+offset)%4); 169 | found = true; 170 | break; 171 | } 172 | } 173 | if (!found) {return;} 174 | 175 | if (nodes[currentNode].contains(prevNode)) {nodes[currentNode].remove(prevNode);} 176 | if (nodes[prevNode].contains(currentNode)) {nodes[prevNode].remove(currentNode);} 177 | 178 | i=currentNode/(2*cols); 179 | j=currentNode%(2*cols); 180 | previ=prevNode/(2*cols); 181 | prevj=prevNode%(2*cols); 182 | PathSequence.add(new Integer[]{previ,prevj,i,j}); 183 | }while (true); 184 | 185 | } 186 | 187 | public ArrayList getPathSequence() {return PathSequence;} 188 | 189 | } 190 | -------------------------------------------------------------------------------- /src/main/java/pathPlanning/darp/ConnectComponent.java: -------------------------------------------------------------------------------- 1 | package pathPlanning.darp; 2 | 3 | import java.awt.*; 4 | 5 | /** 6 | * Use row-by-row labeling algorithm to label connected components 7 | * The algorithm makes two passes over the image: one pass to record 8 | * equivalences and assign temporary labels and the second to replace each 9 | * temporary label by the label of its equivalence class. 10 | * [Reference] 11 | * Linda G. Shapiro, Computer Vision: Theory and Applications. (3.4 Connected 12 | * Components Labeling) 13 | * Rosenfeld and Pfaltz (1966) 14 | */ 15 | public class ConnectComponent 16 | { 17 | int MAX_LABELS; 18 | int next_label = 1; 19 | private int[][] label2d, BinaryRobot, BinaryNonRobot; 20 | private int rows, cols; 21 | 22 | /** 23 | * label and re-arrange the labels to make the numbers of label continous 24 | * @param zeroAsBg Leaving label 0 untouched 25 | */ 26 | public int[][] compactLabeling(int[][] m, Dimension d, boolean zeroAsBg) 27 | { 28 | int[] image =TransformImage2Dto1D(m, d); 29 | label2d = new int[d.height][d.width]; 30 | MAX_LABELS = d.height*d.width; 31 | rows = d.height; 32 | cols = d.width; 33 | //label first 34 | int[] label= labeling(image,d,zeroAsBg); 35 | int[] stat= new int[next_label+1]; 36 | for (int i=0;inext_label) 38 | //System.err.println("bigger label than next_label found!"); 39 | stat[label[i]]++; 40 | } 41 | 42 | stat[0]=0; // label 0 will be mapped to 0 43 | // whether 0 is background or not 44 | int j = 1; 45 | for (int i=1; i 0 && image[y*w+x-1] == image[y*w+x]) { 110 | k = rst[y*w+x-1]; 111 | connected = true; 112 | } 113 | // if connected to the up 114 | if (y > 0 && image[(y-1)*w+x]== image[y*w+x] && (!connected || image[(y-1)*w+x] < k )) { 115 | k = rst[(y-1)*w+x]; 116 | connected = true; 117 | } 118 | if ( !connected ) { 119 | k = next_region; 120 | next_region++; 121 | } 122 | 123 | /* 124 | if ( k >= MAX_LABELS ){ 125 | System.err.println("maximum number of labels reached. " + 126 | "increase MAX_LABELS and recompile." ); 127 | System.exit(1); 128 | }*/ 129 | 130 | rst[y*w+x]= k; 131 | // if connected, but with different label, then do union 132 | if ( x> 0 && image[y*w+x-1]== image[y*w+x] && rst[y*w+x-1]!= k ) 133 | uf_union( k, rst[y*w+x-1], parent ); 134 | if ( y> 0 && image[(y-1)*w+x]== image[y*w+x] && rst[(y-1)*w+x]!= k ) 135 | uf_union( k, rst[(y-1)*w+x], parent ); 136 | } 137 | } 138 | 139 | // Begin the second pass. Assign the new labels 140 | // if 0 is reserved for background, then the first available label is 1 141 | next_label = 1; 142 | for (int i = 0; i < w*h; i++ ) { 143 | if (image[i]!=0 || !zeroAsBg) { 144 | rst[i] = uf_find( rst[i], parent, labels ); 145 | // The labels are from 1, if label 0 should be considered, then 146 | // all the label should minus 1 147 | if (!zeroAsBg) rst[i]--; 148 | } 149 | } 150 | next_label--; // next_label records the max label 151 | if (!zeroAsBg) next_label--; 152 | 153 | //System.out.println(next_label+" regions"); 154 | 155 | return rst; 156 | } 157 | void uf_union( int x, int y, int[] parent) 158 | { 159 | while ( parent[x]>0 ) 160 | x = parent[x]; 161 | while ( parent[y]>0 ) 162 | y = parent[y]; 163 | if ( x != y ) { 164 | if (x0 ) 180 | x = parent[x]; 181 | if ( label[x] == 0 ) 182 | label[x] = next_label++; 183 | return label[x]; 184 | } 185 | 186 | 187 | public void constructBinaryImages(int robotsLabel){ 188 | 189 | BinaryRobot = deepCopyMatrix(label2d); 190 | BinaryNonRobot = deepCopyMatrix(label2d); 191 | 192 | for (int i=0; i Region[y][x]) {minV = Region[y][x];} 252 | } 253 | } 254 | 255 | 256 | //Normalization 257 | for (int y = 0; y < rows; y++) { 258 | for (int x = 0; x < cols; x++) { 259 | if (RobotR) 260 | Region[y][x] = (Region[y][x] - minV) * (1/(maxV-minV)) +1; 261 | else 262 | Region[y][x] = (Region[y][x] - minV) * (1/(maxV-minV)); 263 | } 264 | } 265 | 266 | 267 | return Region; 268 | } 269 | 270 | private void DT1D(float [] f, float [] d, int [] v , float [] z) { 271 | int k = 0; 272 | v[0] = 0; 273 | z[0] = -Float.MAX_VALUE; 274 | z[1] = Float.MAX_VALUE; 275 | 276 | for (int q = 1; q < f.length; q++) { 277 | float s = ((f[q] + q * q) - (f[v[k]] + v[k] * v[k])) / (2 * q - 2 * v[k]); 278 | 279 | while (s <= z[k]) { 280 | k--; 281 | s = ((f[q] + q * q) - (f[v[k]] + v[k] * v[k])) / (2 * q - 2 * v[k]); 282 | } 283 | k++; 284 | v[k] = q; 285 | z[k] = s; 286 | z[k + 1] = Float.MAX_VALUE; 287 | } 288 | 289 | k = 0; 290 | for (int q = 0; q < f.length; q++) { 291 | while (z[k + 1] < q) {k++;} 292 | 293 | d[q] = (q - v[k]) * (q - v[k]) + f[v[k]]; 294 | } 295 | } 296 | 297 | private float[] getVector(float[][] A, int row) { 298 | float[] ret = new float[cols]; 299 | 300 | for (int i=0;i RobotsInit; 17 | private ArrayList BWlist; 18 | private int[][] A; 19 | private boolean[][] robotBinary; 20 | private int[] ArrayOfElements; 21 | private boolean[] ConnectedRobotRegions; 22 | private boolean success; 23 | private ArrayList BinrayRobotRegions; 24 | private int maxCellsAss, minCellsAss; 25 | private double elapsedTime; 26 | private int discr; 27 | private boolean canceled; 28 | private boolean UseImportance; 29 | private double[] Rportions, DesireableAssign; 30 | private int totalIter = 0; 31 | 32 | public DARP(int r, int c, int[][] src, int iters, double vWeight, double rLevel, int discr, boolean imp, double[] Rportions) { 33 | this.rows = r; 34 | this.cols = c; 35 | this.GridEnv = deepCopyMatrix(src); 36 | this.nr = 0; 37 | this.ob = 0; 38 | this.maxIter = iters; 39 | this.RobotsInit = new ArrayList<>(); 40 | this.A = new int[rows][cols]; 41 | this.robotBinary = new boolean[rows][cols]; 42 | this.variateWeight = vWeight; 43 | this.randomLevel = rLevel; 44 | this.discr = discr; 45 | this.canceled = false; 46 | this.UseImportance = imp; 47 | this.Rportions = Rportions; 48 | defineRobotsObstacles(); 49 | } 50 | 51 | public void constructAssignmentM() { 52 | 53 | long startTime = System.nanoTime(); 54 | //Constant Initializations 55 | int NoTiles = rows * cols; 56 | 57 | int termThr; 58 | double fairDivision = 1.0 / nr; 59 | int effectiveSize = NoTiles - nr - ob; 60 | termThr = 0; 61 | 62 | if (effectiveSize % nr != 0) { 63 | termThr = 1; 64 | } 65 | 66 | DesireableAssign = new double[nr]; 67 | for (int i = 0; i < nr; i++) { 68 | DesireableAssign[i] = effectiveSize * Rportions[i]; 69 | if (DesireableAssign[i] != (int) DesireableAssign[i] && termThr!=1){ 70 | termThr = 1; 71 | } 72 | } 73 | 74 | ArrayList AllDistances = new ArrayList<>(); 75 | ArrayList TilesImportance = new ArrayList<>(); 76 | 77 | for (int r = 0; r < nr; r++) { 78 | AllDistances.add(new double[rows][cols]); 79 | TilesImportance.add(new double[rows][cols]); 80 | } 81 | 82 | double[] MaximumDist, MaximumImportance, MinimumImportance; 83 | MaximumDist = new double[nr]; 84 | MaximumImportance = new double[nr]; 85 | MinimumImportance = new double[nr]; 86 | for (int r = 0; r < nr; r++) { 87 | MinimumImportance[r] = Double.MAX_VALUE; 88 | } 89 | 90 | float[][] ONES2D = new float[rows][cols]; 91 | 92 | for (int i = 0; i < rows; i++) { 93 | for (int j = 0; j < cols; j++) { 94 | double tempSum = 0.0; 95 | for (int r = 0; r < nr; r++) { 96 | AllDistances.get(r)[i][j] = EuclideanDis(RobotsInit.get(r), new Integer[]{i, j}); 97 | if (AllDistances.get(r)[i][j] > MaximumDist[r]) { 98 | MaximumDist[r] = AllDistances.get(r)[i][j]; 99 | } 100 | tempSum += AllDistances.get(r)[i][j]; 101 | } 102 | for (int r = 0; r < nr; r++) { 103 | TilesImportance.get(r)[i][j] = 1.0 / (tempSum - AllDistances.get(r)[i][j]); 104 | if (TilesImportance.get(r)[i][j] > MaximumImportance[r]) { 105 | MaximumImportance[r] = TilesImportance.get(r)[i][j]; 106 | } 107 | if (TilesImportance.get(r)[i][j] < MinimumImportance[r]) { 108 | MinimumImportance[r] = TilesImportance.get(r)[i][j]; 109 | } 110 | } 111 | ONES2D[i][j] = 1; 112 | } 113 | } 114 | 115 | success = false; 116 | 117 | ArrayList MetricMatrix = deepCopyListMatrix(AllDistances); 118 | 119 | double[][] criterionMatrix = new double[rows][cols]; 120 | 121 | while (termThr <= discr && !success && !canceled) { 122 | //Initializations 123 | double downThres = ((double) NoTiles - (double) termThr * (nr - 1)) / (double) (NoTiles * nr); 124 | double upperThres = ((double) NoTiles + termThr) / (double) (NoTiles * nr); 125 | 126 | success = true; 127 | //Main optimization loop 128 | int iter = 0; 129 | while (iter <= maxIter && !canceled) { 130 | assign(MetricMatrix); 131 | 132 | ArrayList ConnectedMultiplierList = new ArrayList<>(); 133 | double[] plainErrors = new double[nr]; 134 | double[] divFairError = new double[nr]; 135 | 136 | //Connected Areas Component 137 | for (int r = 0; r < nr; r++) { 138 | float[][] ConnectedMultiplier = deepCopyMatrix(ONES2D); 139 | ConnectedRobotRegions[r] = true; 140 | 141 | ConnectComponent cc = new ConnectComponent(); 142 | int[][] Ilabel = cc.compactLabeling(BWlist.get(r), new Dimension(cols, rows), true); 143 | if (cc.getMaxLabel() > 1) { //At least one unconnected regions among r-robot's regions is found 144 | ConnectedRobotRegions[r] = false; 145 | 146 | //Find robot's sub-region and construct Robot and Non-Robot binary regions 147 | cc.constructBinaryImages(Ilabel[RobotsInit.get(r)[0]][RobotsInit.get(r)[1]]); 148 | 149 | //Construct the final Connected Component Multiplier 150 | ConnectedMultiplier = CalcConnectedMultiplier(cc.NormalizedEuclideanDistanceBinary(true), 151 | cc.NormalizedEuclideanDistanceBinary(false)); 152 | 153 | } 154 | ConnectedMultiplierList.add(r, ConnectedMultiplier); 155 | 156 | //Calculate the deviation from the the Optimal Assignment 157 | //plainErrors[r] = ArrayOfElements[r] / (double) effectiveSize; 158 | plainErrors[r] = ArrayOfElements[r] / (DesireableAssign[r] * nr); 159 | 160 | //System.out.print(ArrayOfElements[r]+ ", "); 161 | //divFairError[r] = fairDivision - plainErrors[r]; 162 | if (plainErrors[r] < downThres) { 163 | divFairError[r] = downThres - plainErrors[r]; 164 | } else if (plainErrors[r] > upperThres) { 165 | divFairError[r] = upperThres - plainErrors[r]; 166 | } 167 | } 168 | 169 | //System.out.print("Iteration: "+iter+", "); 170 | 171 | //Exit conditions 172 | if (isThisAGoalState(termThr)) { 173 | break; 174 | } 175 | 176 | double TotalNegPerc = 0.0, totalNegPlainErrors = 0.0; 177 | double[] correctionMult = new double[nr]; 178 | 179 | for (int r = 0; r < nr; r++) { 180 | if (divFairError[r] < 0) { 181 | TotalNegPerc += Math.abs(divFairError[r]); 182 | totalNegPlainErrors += plainErrors[r]; 183 | } 184 | correctionMult[r] = 1.0; 185 | } 186 | 187 | //Restore Fairness among the different partitions 188 | for (int r = 0; r < nr; r++) { 189 | if (totalNegPlainErrors != 0.0) { 190 | 191 | //correctionMult[r]=1.0 -(divFairError[r]/(TotalNegPerc*nr))*(totalNegPlainErrors/2.0); 192 | 193 | 194 | if (divFairError[r] < 0.0) { 195 | correctionMult[r] = 1.0 + (plainErrors[r] / totalNegPlainErrors) * (TotalNegPerc / 2.0); 196 | } else { 197 | correctionMult[r] = 1.0 - (plainErrors[r] / totalNegPlainErrors) * (TotalNegPerc / 2.0); 198 | } 199 | 200 | criterionMatrix = calculateCriterionMatrix(TilesImportance.get(r), MinimumImportance[r], MaximumImportance[r], correctionMult[r], (divFairError[r] < 0)); 201 | } 202 | MetricMatrix.set(r, FinalUpdateOnMetricMatrix(criterionMatrix, generateRandomMatrix(), MetricMatrix.get(r), ConnectedMultiplierList.get(r))); 203 | } 204 | 205 | iter++; 206 | totalIter++; 207 | } 208 | 209 | 210 | if (iter >= maxIter) { 211 | maxIter = maxIter / 2; 212 | success = false; 213 | termThr++; 214 | } 215 | } 216 | 217 | elapsedTime = (double) (System.nanoTime() - startTime) / Math.pow(10, 9); 218 | calculateRobotBinaryArrays(); 219 | } 220 | 221 | public int getIter() { 222 | return totalIter; 223 | } 224 | 225 | 226 | private void calculateRobotBinaryArrays() { 227 | BinrayRobotRegions = new ArrayList<>(); 228 | for (int r = 0; r < nr; r++) { 229 | BinrayRobotRegions.add(new boolean[rows][cols]); 230 | } 231 | for (int i = 0; i < rows; i++) { 232 | for (int j = 0; j < cols; j++) { 233 | if (A[i][j] < nr) { 234 | BinrayRobotRegions.get(A[i][j])[i][j] = true; 235 | } 236 | } 237 | } 238 | } 239 | 240 | private double[][] FinalUpdateOnMetricMatrix(double[][] CM, double[][] RM, double[][] curentONe, float[][] CC) { 241 | double[][] MMnew = new double[rows][cols]; 242 | 243 | for (int i = 0; i < rows; i++) { 244 | for (int j = 0; j < cols; j++) { 245 | MMnew[i][j] = curentONe[i][j] * CM[i][j] * RM[i][j] * CC[i][j]; 246 | } 247 | } 248 | 249 | return MMnew; 250 | } 251 | 252 | private double[][] generateRandomMatrix() { 253 | 254 | double[][] RandomMa = new double[rows][cols]; 255 | Random randomno = new Random(); 256 | 257 | for (int i = 0; i < rows; i++) { 258 | for (int j = 0; j < cols; j++) { 259 | RandomMa[i][j] = 2.0 * randomLevel * randomno.nextDouble() + 1.0 - randomLevel; 260 | } 261 | } 262 | 263 | return RandomMa; 264 | } 265 | 266 | private double[][] calculateCriterionMatrix(double[][] TilesImp, double minImp, double maxImp, double corMult, boolean SmallerThan0) { 267 | double[][] retrunCriter = new double[rows][cols]; 268 | 269 | for (int i = 0; i < rows; i++) { 270 | for (int j = 0; j < cols; j++) { 271 | if (UseImportance) { 272 | if (SmallerThan0) { 273 | retrunCriter[i][j] = (TilesImp[i][j] - minImp) * ((corMult - 1) / (maxImp - minImp)) + 1; 274 | } else { 275 | retrunCriter[i][j] = (TilesImp[i][j] - minImp) * ((1 - corMult) / (maxImp - minImp)) + corMult; 276 | } 277 | } else { 278 | retrunCriter[i][j] = corMult; 279 | } 280 | } 281 | } 282 | 283 | return retrunCriter; 284 | } 285 | 286 | private boolean isThisAGoalState(int thres) { 287 | // maxCellsAss = 0; 288 | // minCellsAss = Integer.MAX_VALUE; 289 | // 290 | // for (int r = 0; r < nr; r++) { 291 | // if (maxCellsAss < ArrayOfElements[r]) { 292 | // maxCellsAss = ArrayOfElements[r]; 293 | // } 294 | // if (minCellsAss > ArrayOfElements[r]) { 295 | // minCellsAss = ArrayOfElements[r]; 296 | // } 297 | // 298 | // if (!ConnectedRobotRegions[r]) { 299 | // return false; 300 | // } 301 | // } 302 | // 303 | // /* 304 | // System.out.println("Discrepancey: "+(maxCellsAss-minCellsAss)+"("+thres+")"); 305 | // for (int r=0;r thres || !ConnectedRobotRegions[r]) { 316 | return false; 317 | } 318 | } 319 | 320 | // System.out.println("\nFinal assigment"); 321 | // for (int i = 0; i < nr; i++) { 322 | // System.out.println("R"+(i+1)+"--> desired: "+DesireableAssign[i]+" | Converged: "+ArrayOfElements[i]); 323 | // } 324 | // System.out.println(); 325 | 326 | return true; 327 | 328 | } 329 | 330 | private float[][] CalcConnectedMultiplier(float[][] dist1, float[][] dist2) { 331 | float[][] returnM = new float[rows][cols]; 332 | float MaxV = 0; 333 | float MinV = Float.MAX_VALUE; 334 | for (int i = 0; i < rows; i++) { 335 | for (int j = 0; j < cols; j++) { 336 | returnM[i][j] = dist1[i][j] - dist2[i][j]; 337 | if (MaxV < returnM[i][j]) { 338 | MaxV = returnM[i][j]; 339 | } 340 | if (MinV > returnM[i][j]) { 341 | MinV = returnM[i][j]; 342 | } 343 | } 344 | } 345 | 346 | for (int i = 0; i < rows; i++) { 347 | for (int j = 0; j < cols; j++) { 348 | returnM[i][j] = (returnM[i][j] - MinV) * ((2 * (float) variateWeight) / (MaxV - MinV)) + (1 - (float) variateWeight); 349 | } 350 | } 351 | 352 | return returnM; 353 | } 354 | 355 | private void assign(ArrayList Q) { 356 | 357 | BWlist = new ArrayList<>(); 358 | for (int r = 0; r < nr; r++) { 359 | BWlist.add(new int[rows][cols]); 360 | BWlist.get(r)[RobotsInit.get(r)[0]][RobotsInit.get(r)[1]] = 1; 361 | } 362 | 363 | ArrayOfElements = new int[nr]; 364 | for (int i = 0; i < rows; i++) { 365 | for (int j = 0; j < cols; j++) { 366 | if (GridEnv[i][j] == -1) { 367 | double minV = Q.get(0)[i][j]; 368 | int indMin = 0; 369 | for (int r = 1; r < nr; r++) { 370 | if (Q.get(r)[i][j] < minV) { 371 | minV = Q.get(r)[i][j]; 372 | indMin = r; 373 | } 374 | } 375 | A[i][j] = indMin; 376 | BWlist.get(indMin)[i][j] = 1; 377 | ArrayOfElements[indMin]++; 378 | } else if (GridEnv[i][j] == -2) { 379 | A[i][j] = nr; 380 | } 381 | } 382 | } 383 | 384 | } 385 | 386 | private double EuclideanDis(double[] a, double[] b) { 387 | int vecSize = a.length; 388 | double d = 0.0; 389 | for (int i = 0; i < vecSize; i++) { 390 | d += Math.pow(a[i] - b[i], 2.0); 391 | } 392 | return Math.sqrt(d); 393 | } 394 | 395 | private double EuclideanDis(Integer[] a, Integer[] b) { 396 | int vecSize = a.length; 397 | double d = 0.0; 398 | for (int i = 0; i < vecSize; i++) { 399 | d += Math.pow(a[i] - b[i], 2.0); 400 | } 401 | return Math.sqrt(d); 402 | } 403 | 404 | private int[][] deepCopyMatrix(int[][] input) { 405 | if (input == null) 406 | return null; 407 | int[][] result = new int[input.length][]; 408 | for (int r = 0; r < input.length; r++) { 409 | result[r] = input[r].clone(); 410 | } 411 | return result; 412 | } 413 | 414 | private ArrayList deepCopyListMatrix(ArrayList input) { 415 | if (input == null) 416 | return null; 417 | ArrayList result = new ArrayList<>(); 418 | for (int r = 0; r < input.size(); r++) { 419 | result.add(deepCopyMatrix(input.get(r))); 420 | } 421 | return result; 422 | } 423 | 424 | private double[][] deepCopyMatrix(double[][] input) { 425 | if (input == null) 426 | return null; 427 | double[][] result = new double[input.length][]; 428 | for (int r = 0; r < input.length; r++) { 429 | result[r] = input[r].clone(); 430 | } 431 | return result; 432 | } 433 | 434 | private int[] deepCopyMatrix(int[] input) { 435 | if (input == null) 436 | return null; 437 | int[] result = input.clone(); 438 | return result; 439 | } 440 | 441 | private double[] deepCopyMatrix(double[] input) { 442 | if (input == null) 443 | return null; 444 | double[] result = input.clone(); 445 | return result; 446 | } 447 | 448 | private float[][] deepCopyMatrix(float[][] input) { 449 | if (input == null) 450 | return null; 451 | float[][] result = new float[input.length][]; 452 | for (int r = 0; r < input.length; r++) { 453 | result[r] = input[r].clone(); 454 | } 455 | return result; 456 | } 457 | 458 | private void defineRobotsObstacles() { 459 | for (int i = 0; i < rows; i++) { 460 | for (int j = 0; j < cols; j++) { 461 | if (GridEnv[i][j] == 2) { 462 | robotBinary[i][j] = true; 463 | RobotsInit.add(new Integer[]{i, j}); 464 | GridEnv[i][j] = nr; 465 | A[i][j] = nr; 466 | nr++; 467 | } else if (GridEnv[i][j] == 1) { 468 | ob++; 469 | GridEnv[i][j] = -2; 470 | } else { 471 | GridEnv[i][j] = -1; 472 | } 473 | } 474 | } 475 | 476 | ConnectedRobotRegions = new boolean[nr]; 477 | } 478 | 479 | private void printMatrix(int[][] M) { 480 | int r = M.length; 481 | int c = M[0].length; 482 | 483 | System.out.println(); 484 | for (int i = 0; i < r; i++) { 485 | for (int j = 0; j < c; j++) { 486 | System.out.print(M[i][j] + " "); 487 | } 488 | System.out.println(); 489 | } 490 | } 491 | 492 | private void printMatrix(float[][] M) { 493 | int r = M.length; 494 | int c = M[0].length; 495 | 496 | System.out.println(); 497 | for (int i = 0; i < r; i++) { 498 | for (int j = 0; j < c; j++) { 499 | System.out.print(M[i][j] + " "); 500 | } 501 | System.out.println(); 502 | } 503 | } 504 | 505 | private void printMatrix(double[][] M) { 506 | int r = M.length; 507 | int c = M[0].length; 508 | 509 | System.out.println(); 510 | for (int i = 0; i < r; i++) { 511 | for (int j = 0; j < c; j++) { 512 | System.out.print(M[i][j] + " "); 513 | } 514 | System.out.println(); 515 | } 516 | } 517 | 518 | public boolean getSuccess() { 519 | return success; 520 | } 521 | 522 | public int getNr() { 523 | return nr; 524 | } 525 | 526 | public int getNumOB() { 527 | return ob; 528 | } 529 | 530 | public int[][] getAssignmentMatrix() { 531 | return A; 532 | } 533 | 534 | public boolean[][] getRobotBinary() { 535 | return robotBinary; 536 | } 537 | 538 | public ArrayList getBinrayRobotRegions() { 539 | return BinrayRobotRegions; 540 | } 541 | 542 | public ArrayList getRobotsInit() { 543 | return RobotsInit; 544 | } 545 | 546 | public int getEffectiveSize() { 547 | return 4 * (rows * cols - ob); 548 | } 549 | 550 | public int getMaxCellsAss() { 551 | return 4 * (maxCellsAss + 1); 552 | } 553 | 554 | public int getMinCellsAss() { 555 | return 4 * (minCellsAss + 1); 556 | } 557 | 558 | public double getElapsedTime() { 559 | return elapsedTime; 560 | } 561 | 562 | public int getDiscr() { 563 | return discr; 564 | } 565 | 566 | public int getMaxIter() { 567 | return maxIter; 568 | } 569 | 570 | public void setCanceled(boolean c) { 571 | this.canceled = c; 572 | } 573 | 574 | public int getAchievedDiscr() { 575 | return maxCellsAss - minCellsAss; 576 | } 577 | 578 | } 579 | -------------------------------------------------------------------------------- /src/main/java/pathPlanning/darp/DARPinPoly.java: -------------------------------------------------------------------------------- 1 | package pathPlanning.darp; 2 | 3 | import pathPlanning.handleGeo.*; 4 | import pathPlanning.nodesPlacementOptimization.*; 5 | 6 | import java.awt.*; 7 | import java.util.ArrayList; 8 | import java.util.Random; 9 | import java.util.Vector; 10 | 11 | 12 | public class DARPinPoly { 13 | 14 | private ArrayList> missionWaypointsNED = new ArrayList<>(); 15 | private ArrayList> missionWaypointsWGS84 = new ArrayList<>(); 16 | private int droneNo; 17 | private boolean pathsStrictlyInPoly; 18 | private int[][] DARPgrid; 19 | private double[][] geo; 20 | 21 | 22 | public void waypointsDARP(int droneNo, int scanDist, double[][] geo, double[][][] obstacles, boolean pathsStrictlyInPoly) { 23 | common(droneNo, scanDist, geo, obstacles, pathsStrictlyInPoly, new double[][]{}, true, new double[]{}, false); 24 | } 25 | 26 | public void waypointsDARP(int droneNo, int scanDist, double[][] geo, double[][][] obstacles, boolean pathsStrictlyInPoly, double[][] initialPos) { 27 | common(droneNo, scanDist, geo, obstacles, pathsStrictlyInPoly, initialPos, false, new double[]{}, false); 28 | } 29 | 30 | public void waypointsDARP(int droneNo, int scanDist, double[][] geo, double[][][] obstacles, boolean pathsStrictlyInPoly, double[] Rportions) { 31 | common(droneNo, scanDist, geo, obstacles, pathsStrictlyInPoly, new double[][]{}, true, Rportions, true); 32 | } 33 | 34 | public void waypointsDARP(int droneNo, int scanDist, double[][] geo, double[][][] obstacles, boolean pathsStrictlyInPoly, double[][] initialPos, double[] Rportions) { 35 | common(droneNo, scanDist, geo, obstacles, pathsStrictlyInPoly, initialPos, false, Rportions, true); 36 | } 37 | 38 | private void common(int droneNo, int scanDist, double[][] geo, double[][][] obstacles, boolean pathsStrictlyInPoly, double[][] initialPos, boolean randomInitPos, double[] Rportions, boolean notEqualPortions) { 39 | 40 | this.droneNo = droneNo; 41 | this.pathsStrictlyInPoly = pathsStrictlyInPoly; 42 | this.geo = geo; 43 | 44 | long start1 = System.nanoTime(); 45 | 46 | // Convert WGS84 to NED coordinates 47 | ConvCoords missionDARP = new ConvCoords(); 48 | missionDARP.polygonWGS84ToNED(geo); // Convert geographical to local cartesian coordinates 49 | double[][] cartUnrotated = missionDARP.getPolygonNED(); 50 | double[][][] obstNED = new double[][][]{}; 51 | if (obstacles.length > 0) { 52 | missionDARP.obstaclesToNED(obstacles); 53 | obstNED = missionDARP.getObstaclesNED(); 54 | } 55 | 56 | // Rotation and shift optimization 57 | long start = System.nanoTime(); 58 | SimulatedAnnealing optimalParameters = new SimulatedAnnealing(); 59 | optimalParameters.run(cartUnrotated, obstNED, scanDist); 60 | Rotate rotate = new Rotate(); 61 | float theta = optimalParameters.getOptimalTheta(); 62 | float shiftX = optimalParameters.getOptimalShiftX(); 63 | float shiftY = optimalParameters.getOptimalShiftY(); 64 | rotate.setTheta(theta); 65 | double[][] cart = rotate.rotatePolygon(cartUnrotated); 66 | double[][][] cartObst = new double[obstNED.length][][]; 67 | for (int i=0; i 1) { 122 | System.out.println("\n\n !!! The environment grid MUST not have unreachable and/or closed shape regions !!! \n\n"); 123 | return; 124 | } 125 | 126 | // Put drones in initial positions (close to physical or random) 127 | initializeDARPGrid(randomInitPos, l, m, initialPos, missionDARP, megaNodes, theta, shiftX, shiftY); 128 | 129 | // Parameters to run DARP 130 | int MaxIter = 80000; 131 | double CCvariation = 0.01; 132 | double randomLevel = 0.0001; 133 | int dcells = 2; 134 | boolean importance = false; 135 | 136 | // If user has not defined custom portions divide area equally for all drones 137 | if (!notEqualPortions) { 138 | Rportions = new double[droneNo]; 139 | for (int i = 0; i < droneNo; i++) { 140 | Rportions[i] = 1.0/droneNo; 141 | } 142 | } 143 | 144 | System.out.println("Portion of the total area assigned to each drone:"); 145 | for (int i = 0; i < droneNo; i++) { 146 | System.out.println(" - "+Rportions[i]*100+" %"); 147 | } 148 | System.out.println(); 149 | 150 | // Perform operational area division (run DARP) 151 | DARP problem = new DARP(l, m, DARPgrid, MaxIter, CCvariation, randomLevel, dcells, importance, Rportions); 152 | 153 | // Warn when no drone is defined 154 | System.out.println("Number of drones: " + problem.getNr()); 155 | if (problem.getNr() <= 0) { 156 | System.out.println("\n\n !!! You should use at least one drone !!!\n"); 157 | return; 158 | } 159 | 160 | // Check if DARP could find a solution - if not --> rerun for random initial positions 161 | int[][] DARPAssignmentMatrix = new int[0][]; 162 | problem.constructAssignmentM(); 163 | if (problem.getSuccess()) { 164 | DARPAssignmentMatrix = problem.getAssignmentMatrix(); 165 | } else { 166 | int count = 0; 167 | while (!problem.getSuccess() && count < 5) { 168 | count++; 169 | System.out.println("\n\n !!! DARP will rerun for random initial positions !!! \n"); 170 | 171 | DARPgrid = new int[l][m]; 172 | for (int i = 0; i < l; i++) { 173 | for (int j = 0; j < m; j++) { 174 | DARPgrid[i][j] = (int) megaNodes[i][j][2]; 175 | } 176 | } 177 | initializeDARPGrid(true, l, m, initialPos, missionDARP, megaNodes, theta, shiftX, shiftY); 178 | 179 | problem = new DARP(l, m, DARPgrid, MaxIter, CCvariation, randomLevel, dcells, importance, Rportions); 180 | problem.constructAssignmentM(); 181 | DARPAssignmentMatrix = problem.getAssignmentMatrix(); 182 | } 183 | if (count == 5) { 184 | System.out.println("\nDARP did not manage to find a solution for the given configuration!\n Try to alter one or more of:" + 185 | "\n - Scanning distance\n - Number of drones \n - Given area\n\n"); 186 | return; 187 | } 188 | } 189 | 190 | // Calculate paths for all drones, for all modes (see below) and keep the paths with the minimum turns 191 | ArrayList>[] allDirectionsWaypoints = new ArrayList[droneNo]; 192 | for (int k = 0; k < droneNo; k++) { 193 | allDirectionsWaypoints[k] = new ArrayList<>(); 194 | } 195 | for (int mode = 0; mode < 4; mode++) { 196 | 197 | // Calculate MSTs for all drones 198 | // ------------------------------------------------------------------------------------------ // 199 | // mode is a variable that configures the side that the branches of the MST will be connected // 200 | // this variable is used to minimize the turn points for every drone // 201 | // 0 - connection on top // 202 | // 1 - connection on bottom // 203 | // 2 - connection on right // 204 | // 3 - connection on left // 205 | // ------------------------------------------------------------------------------------------ // 206 | ArrayList MSTs = calculateMSTs(problem.getBinrayRobotRegions(), droneNo, l, m, mode); 207 | 208 | // Find paths around MSTs (circumnavigation) 209 | ArrayList InitRobots = problem.getRobotsInit(); // Initial positions of drones 210 | ArrayList> AllRealPaths = new ArrayList<>(); 211 | for (int r = 0; r < droneNo; r++) { 212 | CalculateTrajectories ct = new CalculateTrajectories(l, m, MSTs.get(r)); //Send MSTs 213 | ct.initializeGraph(CalcRealBinaryReg(problem.getBinrayRobotRegions().get(r), l, m), true); //Send [x2 x2] Binary Robot Region 214 | ct.RemoveTheAppropriateEdges(); 215 | ct.CalculatePathsSequence(4 * InitRobots.get(r)[0] * m + 2 * InitRobots.get(r)[1]); 216 | AllRealPaths.add(ct.getPathSequence()); 217 | } 218 | 219 | // --------------------------------------------------------------------------------------------------------- 220 | // Type of lines is a 2*l x 2*m x 2 matrix - [2*l][2*m][0] contains the clockwise path 221 | // [2*l][2*m][1] contains the counterclockwise path 222 | // --------------------------------------------------------------------------------------------------------- 223 | // In both matrices: 0 stands for obstacle or starting ending points 224 | // 1 stands for up movement 225 | // 2 stands for left movement 226 | // 3 stands for right movement 227 | // 4 stands for down movement 228 | // --------------------------------------------------------------------------------------------------------- 229 | int[][][] TypesOfLines = new int[l * 2][m * 2][2]; 230 | int indxadd1; 231 | int indxadd2; 232 | 233 | for (int r = 0; r < droneNo; r++) { 234 | boolean flag = false; // This variable is used to fix a bug that came along with darp 235 | for (Integer[] connection : AllRealPaths.get(r)) { 236 | if (flag) { 237 | if (TypesOfLines[connection[0]][connection[1]][0] == 0) { 238 | indxadd1 = 0; 239 | } else { 240 | indxadd1 = 1; 241 | } 242 | if (TypesOfLines[connection[2]][connection[3]][0] == 0 && flag) { 243 | indxadd2 = 0; 244 | } else { 245 | indxadd2 = 1; 246 | } 247 | } else { 248 | if (!(TypesOfLines[connection[0]][connection[1]][0] == 0)) { 249 | indxadd1 = 0; 250 | } else { 251 | indxadd1 = 1; 252 | } 253 | if (!(TypesOfLines[connection[2]][connection[3]][0] == 0 && flag)) { 254 | indxadd2 = 0; 255 | } else { 256 | indxadd2 = 1; 257 | } 258 | } 259 | 260 | flag = true; 261 | 262 | if (connection[0].equals(connection[2])) { // Horizontal connection (Line types: 2,3) 263 | if (connection[1] > connection[3]) { 264 | TypesOfLines[connection[0]][connection[1]][indxadd1] = 2; 265 | TypesOfLines[connection[2]][connection[3]][indxadd2] = 3; 266 | } else { 267 | TypesOfLines[connection[0]][connection[1]][indxadd1] = 3; 268 | TypesOfLines[connection[2]][connection[3]][indxadd2] = 2; 269 | } 270 | } else { // Vertical connection (Line types: 1,4) 271 | if (connection[0] > connection[2]) { 272 | TypesOfLines[connection[0]][connection[1]][indxadd1] = 1; 273 | TypesOfLines[connection[2]][connection[3]][indxadd2] = 4; 274 | } else { 275 | TypesOfLines[connection[0]][connection[1]][indxadd1] = 4; 276 | TypesOfLines[connection[2]][connection[3]][indxadd2] = 1; 277 | } 278 | } 279 | } 280 | } 281 | 282 | // Clockwise path 283 | // System.out.println(); 284 | for (int i = 0; i < TypesOfLines.length; i++) { 285 | for (int j = 0; j < TypesOfLines[0].length; j++) { 286 | subNodes[i][j][2] = TypesOfLines[i][j][0]; 287 | // System.out.print((int)subNodes[i][j][2]+" "); 288 | } 289 | // System.out.println(); 290 | } 291 | 292 | // // Counterclockwise path 293 | // System.out.println(); 294 | // for (int i = 0; i < TypesOfLines.length; i++) { 295 | // for (int j = 0; j < TypesOfLines[0].length; j++) { 296 | // subNodes[i][j][2] = TypesOfLines[i][j][1]; 297 | // System.out.print((int)subNodes[i][j][2]+" "); 298 | // } 299 | // System.out.println(); 300 | // } 301 | // ------------------------------------------------------------------------------------------------------------ 302 | 303 | // From TypesOfLines to lists of waypoints 304 | int xInit; // Coordinates for initial position 305 | int yInit; // of drones in the sub-cells 306 | 307 | int[][] subCellsAssignment = new int[l * 2][m * 2]; 308 | for (int i = 0; i < l; i++) { 309 | for (int j = 0; j < m; j++) { 310 | subCellsAssignment[2 * i][2 * j] = DARPAssignmentMatrix[i][j]; 311 | subCellsAssignment[2 * i + 1][2 * j] = DARPAssignmentMatrix[i][j]; 312 | subCellsAssignment[2 * i][2 * j + 1] = DARPAssignmentMatrix[i][j]; 313 | subCellsAssignment[2 * i + 1][2 * j + 1] = DARPAssignmentMatrix[i][j]; 314 | } 315 | } 316 | 317 | ArrayList iWaypoints; 318 | double prevState; 319 | int i; 320 | int j; 321 | 322 | for (int k = 0; k < droneNo; k++) { 323 | iWaypoints = new ArrayList<>(); 324 | // Compute waypoints for every drone 325 | xInit = 2 * InitRobots.get(k)[0]; 326 | yInit = 2 * InitRobots.get(k)[1]; 327 | // System.out.println("Initial positions: "+xInit+","+ yInit); 328 | 329 | i = xInit; 330 | j = yInit; 331 | 332 | iWaypoints.add(new double[]{subNodes[xInit][yInit][0], subNodes[xInit][yInit][1]}); 333 | do { 334 | 335 | prevState = subNodes[i][j][2]; 336 | 337 | if (subNodes[i][j][2] == 1.0) { 338 | i--; 339 | } else if (subNodes[i][j][2] == 2.0) { 340 | j--; 341 | } else if (subNodes[i][j][2] == 3.0) { 342 | j++; 343 | } else if (subNodes[i][j][2] == 4.0) { 344 | i++; 345 | } 346 | 347 | if (prevState != subNodes[i][j][2] || (subNodes[i][j][0] == iWaypoints.get(0)[0] && subNodes[i][j][1] == iWaypoints.get(0)[1])) { 348 | iWaypoints.add(new double[]{subNodes[i][j][0], subNodes[i][j][1]}); 349 | } 350 | 351 | } while (!(i == xInit && j == yInit)); 352 | ArrayList WP = rotate.rotateBackWaypoints(iWaypoints); 353 | allDirectionsWaypoints[k].add(WP); 354 | } 355 | } 356 | 357 | // Keep orientation with less turns 358 | int ind; 359 | int min; 360 | for (int i = 0; i < droneNo; i++) { 361 | ind = 0; 362 | min = allDirectionsWaypoints[i].get(0).size(); 363 | for (int j = 1; j < 4; j++) { 364 | if (allDirectionsWaypoints[i].get(j).size() < min) { 365 | min = allDirectionsWaypoints[i].get(j).size(); 366 | ind = j; 367 | } 368 | } 369 | missionWaypointsNED.add(allDirectionsWaypoints[i].get(ind)); 370 | } 371 | missionDARP.NEDToWGS84(missionWaypointsNED); 372 | missionWaypointsWGS84 = missionDARP.getWaypointsWGS84(); 373 | 374 | printWaypointsNum(missionWaypointsWGS84); 375 | 376 | System.out.println("Overall execution time: " + (System.nanoTime() - start1) / 1000000000.0f + " seconds"); 377 | 378 | // printWaypointsNED(); 379 | // printWaypointsWGS84(); 380 | } 381 | 382 | private void initializeDARPGrid(boolean randomInitPos, int l, int m, double[][] initialPos, ConvCoords missionDARP, double[][][] megaNodes, float theta, float shiftX, float shiftY) { 383 | if (randomInitPos) { 384 | 385 | // Add drones in random initial positions 386 | int i1; 387 | int i2; 388 | int c = 0; 389 | while (c < droneNo) { 390 | // Initial positions of drones 391 | Random ind1 = new Random(); 392 | i1 = ind1.nextInt(l); 393 | Random ind2 = new Random(); 394 | i2 = ind2.nextInt(m); 395 | if (DARPgrid[i1][i2] == 0) { 396 | DARPgrid[i1][i2] = 2; 397 | c++; 398 | } 399 | } 400 | 401 | } else { 402 | 403 | // Add drones in the closest mega-node 404 | int i1 = 0; 405 | int i2 = 0; 406 | 407 | // Convert initial positions from WGS84 to NED 408 | double[][] initPosNED = missionDARP.convWGS84ToNED(initialPos); 409 | 410 | // Rotate and shift intial positions 411 | Rotate rotate = new Rotate(); 412 | rotate.setTheta(theta); 413 | double[][] initialPosNED = rotate.rotatePolygon(initPosNED); 414 | for (int i=0; i calculateMSTs(ArrayList BinrayRobotRegions, int nr, int rows, int cols, int mode) { 439 | ArrayList MSTs = new ArrayList<>(); 440 | for (int r = 0; r < nr; r++) { 441 | Kruskal k = new Kruskal(rows * cols); 442 | k.initializeGraph(BinrayRobotRegions.get(r), true, mode); 443 | k.performKruskal(); 444 | MSTs.add(k.getAllNewEdges()); 445 | } 446 | 447 | return MSTs; 448 | } 449 | 450 | private boolean[][] CalcRealBinaryReg(boolean[][] BinrayRobotRegion, int rows, int cols) { 451 | boolean[][] RealBinrayRobotRegion = new boolean[2 * rows][2 * cols]; 452 | for (int i = 0; i < 2 * rows; i++) { 453 | for (int j = 0; j < 2 * cols; j++) { 454 | RealBinrayRobotRegion[i][j] = BinrayRobotRegion[i / 2][j / 2]; 455 | } 456 | } 457 | return RealBinrayRobotRegion; 458 | } 459 | 460 | private void print(ArrayList> waypoints) { 461 | for (int k = 0; k < droneNo; k++) { 462 | System.out.println("\n ~ Number of Waypoints: " + waypoints.get(k).size() + " ~ "); 463 | for (int jj = 0; jj < waypoints.get(k).size(); jj++) { 464 | System.out.println(waypoints.get(k).get(jj)[0] + ", " + waypoints.get(k).get(jj)[1] + ";"); 465 | } 466 | } 467 | } 468 | 469 | private void printWaypointsNum(ArrayList> waypoints) { 470 | for (int k = 0; k < droneNo; k++) { 471 | System.out.println("- Number of Waypoints for drone " + (k + 1) + ": " + waypoints.get(k).size()); 472 | } 473 | System.out.println(); 474 | } 475 | 476 | 477 | public ArrayList> getMissionWaypointsNED() { 478 | return missionWaypointsNED; 479 | } 480 | 481 | public ArrayList> getMissionWaypointsWGS84() { 482 | return missionWaypointsWGS84; 483 | } 484 | 485 | public void printWaypointsNED() { 486 | print(missionWaypointsNED); 487 | } 488 | 489 | public void printWaypointsWGS84() { 490 | print(missionWaypointsWGS84); 491 | } 492 | 493 | } 494 | -------------------------------------------------------------------------------- /src/main/java/pathPlanning/darp/Edge.java: -------------------------------------------------------------------------------- 1 | package pathPlanning.darp; 2 | 3 | import java.util.Comparator; 4 | 5 | /** 6 | * Created by atkap on 5/24/2016. 7 | */ 8 | 9 | class Edge implements Comparator { 10 | // Inner class for representing edge+end-points 11 | public int from, to, cost; 12 | public Edge() { 13 | // Default constructor for TreeSet creation 14 | } 15 | public Edge(int f, int t, int c) { 16 | // Inner class constructor 17 | from = f; to = t; cost = c; 18 | } 19 | 20 | public int compare(Object o1, Object o2) { 21 | // Used for comparisions during add/remove operations 22 | int cost1 = ((Edge) o1).cost; 23 | int cost2 = ((Edge) o2).cost; 24 | int from1 = ((Edge) o1).from; 25 | int from2 = ((Edge) o2).from; 26 | int to1 = ((Edge) o1).to; 27 | int to2 = ((Edge) o2).to; 28 | 29 | if (cost1cost2) 36 | return(1); 37 | else 38 | return(0); 39 | } 40 | 41 | public int hashCode(){ 42 | return (Integer.toString(cost)+Integer.toString(from)+Integer.toString(to)).hashCode(); 43 | } 44 | 45 | public boolean equals(Object obj) { 46 | // Used for comparisions during add/remove operations 47 | Edge e = (Edge) obj; 48 | return (cost==e.cost && from==e.from && to==e.to); 49 | } 50 | 51 | } 52 | -------------------------------------------------------------------------------- /src/main/java/pathPlanning/darp/Kruskal.java: -------------------------------------------------------------------------------- 1 | package pathPlanning.darp; 2 | 3 | import java.util.HashSet; 4 | import java.util.TreeSet; 5 | import java.util.Vector; 6 | 7 | /** 8 | * Created by atkap on 5/20/2016. 9 | */ 10 | 11 | public class Kruskal { 12 | private HashSet nodes[]; // Array of connected components 13 | private TreeSet allEdges; // Priority queue of Edge objects 14 | private Vector allNewEdges; // Edges in Minimal-Spanning Tree 15 | private int MAX_NODES; 16 | 17 | Kruskal(int maxN) { 18 | // Constructor 19 | this.MAX_NODES = maxN; 20 | nodes = new HashSet[MAX_NODES]; // Create array for components 21 | allEdges = new TreeSet(new Edge()); // Create empty priority queue 22 | allNewEdges = new Vector(MAX_NODES); // Create vector for MST edges 23 | } 24 | 25 | public void initializeGraph(boolean [][] A, boolean connect4, int mode) { 26 | int rows = A.length; 27 | int cols = A[0].length; 28 | int cost1 = 1; 29 | int cost2 = 1; 30 | 31 | for (int i=0;i0 && A[i-1][j]) {AddToAllEdges(i*cols+j, (i-1)*cols+j, cost1);} 46 | if (i0 && A[i][j-1]) {AddToAllEdges(i*cols+j, i*cols+j-1, cost2);} 48 | if (j0 && j>0 && A[i-1][j-1]) {AddToAllEdges(i*cols+j, (i-1)*cols+j-1, 1);} 52 | if (irows-1 && j>0 && A[i+1][j-1]) {AddToAllEdges(i*cols+j, (i+1)*cols+j-1, 1);} 54 | if (i>0 && j nodes[curEdge.to].size()) { 91 | // have to transfer all nodes including curEdge.to 92 | src = nodes[curEdge.to]; 93 | dst = nodes[dstHashSetIndex = curEdge.from]; 94 | } else { 95 | // have to transfer all nodes including curEdge.from 96 | src = nodes[curEdge.from]; 97 | dst = nodes[dstHashSetIndex = curEdge.to]; 98 | } 99 | 100 | Object srcArray[] = src.toArray(); 101 | int transferSize = srcArray.length; 102 | for (int j=0; j> waypointsWGS84; 15 | private ArrayList> waypointsNED; 16 | 17 | public void polygonWGS84ToNED(double[][] geoCoords){ 18 | polygonWGS84 = geoCoords; 19 | reference = new WGS84(Math.toRadians(geoCoords[0][0]), Math.toRadians(geoCoords[0][1]), 0); 20 | 21 | polygonNED = WGS84toNED(geoCoords); 22 | } 23 | 24 | public double[][] convWGS84ToNED(double[][] geoCoords){ 25 | double[][] cartCoords = new double[geoCoords.length][geoCoords[0].length]; 26 | cartCoords = WGS84toNED(geoCoords); 27 | 28 | return cartCoords; 29 | } 30 | 31 | public void obstaclesToNED(double[][][] geoObstacles){ 32 | obstaclesWGS84 = geoObstacles; 33 | obstaclesNED = new double[geoObstacles.length][][]; 34 | for (int i=0; i> cartCoords){ 55 | WGS84 wgs84; 56 | NED ned; 57 | ArrayList> local = new ArrayList<>(); 58 | 59 | waypointsNED = cartCoords; 60 | for (int j=0; j geoCoords = new ArrayList<>(); 62 | for (int i=0; i> getWaypointsWGS84(){ 89 | return waypointsWGS84; 90 | } 91 | 92 | public ArrayList> getWaypointsNED(){ 93 | return waypointsNED; 94 | } 95 | 96 | } -------------------------------------------------------------------------------- /src/main/java/pathPlanning/handleGeo/Dist.java: -------------------------------------------------------------------------------- 1 | package pathPlanning.handleGeo; 2 | 3 | import java.util.ArrayList; 4 | 5 | public class Dist { 6 | 7 | public static double geo(double[] p1, double[] p2){ 8 | double distance = 0; 9 | final int R = 6371; // Radius of the earth 10 | 11 | Double latDistance = Math.toRadians(p2[0] - p1[0]); 12 | Double lonDistance = Math.toRadians(p2[1] - p1[1]); 13 | Double a = Math.sin(latDistance / 2) * Math.sin(latDistance / 2) 14 | + Math.cos(Math.toRadians(p1[0])) * Math.cos(Math.toRadians(p2[0])) 15 | * Math.sin(lonDistance / 2) * Math.sin(lonDistance / 2); 16 | Double c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1 - a)); 17 | distance = R * c * 1000; // convert to meters 18 | 19 | return distance; 20 | } 21 | 22 | public static double euclidean(double[] x1, double[] x2){ 23 | double distance = -1; 24 | distance = Math.sqrt((x2[1] - x1[1])*(x2[1] - x1[1]) + (x2[0] - x1[0])*(x2[0] - x1[0])); 25 | 26 | return distance; 27 | } 28 | 29 | public static double getLength(ArrayList WGS84, boolean printLength){ 30 | 31 | double dist = 0; 32 | for (int i=0; i WGS84, float speed){ 42 | double dist = 0; 43 | for (int i=0; i p, q and r are colinear 71 | // 1 --> Clockwise 72 | // 2 --> Counterclockwise 73 | static int orientation(Point p, Point q, Point r) { 74 | double val = (q.Lon - p.Lon) * (r.Lat - q.Lat) - (q.Lat - p.Lat) * (r.Lon - q.Lon); 75 | if (val == 0.0) return 0; // colinear 76 | return (val > 0.0) ? 1 : 2; // clock or counterclock wise 77 | } 78 | 79 | // Given three colinear points p, q, r, the function checks if point q lies on line segment 'pr' 80 | static boolean onSegment(Point p, Point q, Point r) { 81 | if (q.Lat <= Double.max(p.Lat, r.Lat) && q.Lat >= Double.min(p.Lat, r.Lat) && 82 | q.Lon <= Double.max(p.Lon, r.Lon) && q.Lon >= Double.min(p.Lon, r.Lon)) { 83 | return true; 84 | } 85 | return false; 86 | } 87 | 88 | public static class Point { 89 | public double Lat; 90 | public double Lon; 91 | 92 | public Point(double Lat, double Lon) { 93 | this.Lat = Lat; 94 | this.Lon = Lon; 95 | } 96 | } 97 | 98 | } -------------------------------------------------------------------------------- /src/main/java/pathPlanning/handleGeo/NodesInPoly.java: -------------------------------------------------------------------------------- 1 | package pathPlanning.handleGeo; 2 | 3 | import pathPlanning.darp.ConnectComponent; 4 | import pathPlanning.helpers.MinMax; 5 | 6 | import java.awt.*; 7 | 8 | //import java.util.concurrent.Executor; 9 | //import java.util.concurrent.Executors; 10 | //import java.util.concurrent.ScheduledExecutorService; 11 | 12 | public class NodesInPoly { 13 | 14 | private double[][] polygonCoordinates; 15 | private double[][][] cartObst; 16 | 17 | private double shiftX; 18 | private double shiftY; 19 | 20 | private double xRngMeters; 21 | private double yRngMeters; 22 | 23 | private int xNodes; 24 | private int yNodes; 25 | private int megaNodesCount; 26 | 27 | private double xMax; 28 | private double xMin; 29 | private double yMax; 30 | private double yMin; 31 | 32 | private double[][][] megaNodes; 33 | private double[][][] subNodes; 34 | 35 | private int nodeDistance; 36 | private float nodeIntervalOffset; 37 | 38 | // ScheduledExecutorService executor = Executors.newScheduledThreadPool(10); 39 | 40 | public NodesInPoly(double[][] cartCoords, double[][][] cartObst, int scanDist, boolean pathsStrictlyInPoly, boolean hideInfo, double shiftX, double shiftY) { 41 | 42 | this.cartObst = cartObst; 43 | this.shiftX = shiftX; 44 | this.shiftY = shiftY; 45 | nodeDistance = 2 * scanDist; 46 | nodeIntervalOffset = scanDist / 2.0f; 47 | polygonCoordinates = cartCoords; 48 | 49 | xMax = MinMax.xMax(polygonCoordinates) + nodeDistance; 50 | xMin = MinMax.xMin(polygonCoordinates) - nodeDistance; 51 | yMax = MinMax.yMax(polygonCoordinates) + nodeDistance; 52 | yMin = MinMax.yMin(polygonCoordinates) - nodeDistance; 53 | 54 | xRngMeters = xMax - xMin; 55 | yRngMeters = yMax - yMin; 56 | 57 | xNodes = (int) (xRngMeters / nodeDistance); 58 | yNodes = (int) (yRngMeters / nodeDistance); 59 | 60 | if (!hideInfo) { 61 | System.out.println("Bounding box: " + xRngMeters + " x " + yRngMeters + " meters"); 62 | System.out.println("xNodes: " + xNodes); 63 | System.out.println("yNodes: " + yNodes); 64 | System.out.println("Total number of mega-nodes: " + xNodes * yNodes); 65 | } 66 | 67 | double xInter = xMax - xMin - (xNodes - 1) * nodeDistance; 68 | double yInter = yMax - yMin - (yNodes - 1) * nodeDistance; 69 | 70 | if (pathsStrictlyInPoly) { 71 | strictlyInPoly(xMin, yMin, nodeDistance, hideInfo); 72 | } else { 73 | betterCoverage(xMin, yMin, nodeDistance, hideInfo); 74 | } 75 | 76 | // removeObstacles(); 77 | 78 | } 79 | 80 | private void strictlyInPoly(double xMin, double yMin, int nodeDistance, boolean hideInfo) { 81 | 82 | megaNodesCount = 0; 83 | megaNodes = new double[xNodes][yNodes][3]; 84 | for (int i = 0; i < xNodes; i++) { 85 | for (int j = 0; j < yNodes; j++) { 86 | megaNodes[i][j][0] = xMin + i * nodeDistance + shiftX; 87 | megaNodes[i][j][1] = yMin + j * nodeDistance + shiftY; 88 | // // Uncomment to print the mega-megaNodes 89 | // if (!hideInfo){ 90 | // System.out.println(megaNodes[i][j][0]+", "+megaNodes[i][j][1]+" ---> "+megaNodes[i][j][2]); 91 | // } 92 | } 93 | } 94 | 95 | subNodes = new double[2 * xNodes][2 * yNodes][3]; 96 | for (int i = 0; i < xNodes; i++) { 97 | for (int j = 0; j < yNodes; j++) { 98 | 99 | subNodes[2 * i][2 * j + 1][0] = megaNodes[i][j][0] - nodeIntervalOffset; 100 | subNodes[2 * i][2 * j + 1][1] = megaNodes[i][j][1] + nodeIntervalOffset; 101 | 102 | subNodes[2 * i + 1][2 * j + 1][0] = megaNodes[i][j][0] + nodeIntervalOffset; 103 | subNodes[2 * i + 1][2 * j + 1][1] = megaNodes[i][j][1] + nodeIntervalOffset; 104 | 105 | subNodes[2 * i][2 * j][0] = megaNodes[i][j][0] - nodeIntervalOffset; 106 | subNodes[2 * i][2 * j][1] = megaNodes[i][j][1] - nodeIntervalOffset; 107 | 108 | subNodes[2 * i + 1][2 * j][0] = megaNodes[i][j][0] + nodeIntervalOffset; 109 | subNodes[2 * i + 1][2 * j][1] = megaNodes[i][j][1] - nodeIntervalOffset; 110 | 111 | if (InPolygon.check(new double[]{subNodes[2 * i][2 * j + 1][0], subNodes[2 * i][2 * j + 1][1]}, polygonCoordinates) && 112 | InPolygon.check(new double[]{subNodes[2 * i + 1][2 * j + 1][0], subNodes[2 * i + 1][2 * j + 1][1]}, polygonCoordinates) && 113 | InPolygon.check(new double[]{subNodes[2 * i][2 * j][0], subNodes[2 * i][2 * j][1]}, polygonCoordinates) && 114 | InPolygon.check(new double[]{subNodes[2 * i + 1][2 * j][0], subNodes[2 * i + 1][2 * j][1]}, polygonCoordinates)) { 115 | 116 | megaNodes[i][j][2] = 0; 117 | megaNodesCount++; 118 | checkInPolyAndObstacle(i, j); 119 | 120 | } else { 121 | megaNodes[i][j][2] = 1; 122 | } 123 | subNodes[2 * i][2 * j + 1][2] = megaNodes[i][j][2]; 124 | subNodes[2 * i + 1][2 * j + 1][2] = megaNodes[i][j][2]; 125 | subNodes[2 * i][2 * j][2] = megaNodes[i][j][2]; 126 | subNodes[2 * i + 1][2 * j][2] = megaNodes[i][j][2]; 127 | 128 | // // Uncomment to print all the sub-nodes 129 | // System.out.println(subNodes[2*i+1][2*j][0]+", "+subNodes[2*i][2*j][1]+" ---> "+subNodes[2*i][2*j][2]); 130 | // System.out.println(subNodes[2*i+1][2*j][0]+", "+subNodes[2*i+1][2*j][1]+" ---> "+subNodes[2*i+1][2*j][2]); 131 | // System.out.println(subNodes[2*i][2*j+1][0]+", "+subNodes[2*i][2*j+1][1]+" ---> "+subNodes[2*i][2*j+1][2]); 132 | // System.out.println(subNodes[2*i+1][2*j+1][0]+", "+subNodes[2*i+1][2*j+1][1]+" ---> "+subNodes[2*i+1][2*j+1][2]); 133 | // 134 | // // Uncomment to print the sub-nodes that will be used for trajectories 135 | // if (megaNodes[i][j][2]!=1){ 136 | // System.out.println(subNodes[2*i][2*j][0]+", "+subNodes[2*i][2*j][1]); 137 | // System.out.println(subNodes[2*i+1][2*j][0]+", "+subNodes[2*i+1][2*j][1]); 138 | // System.out.println(subNodes[2*i][2*j+1][0]+", "+subNodes[2*i][2*j+1][1]); 139 | // System.out.println(subNodes[2*i+1][2*j+1][0]+", "+subNodes[2*i+1][2*j+1][1]); 140 | // } 141 | 142 | } 143 | 144 | } 145 | 146 | if (!hideInfo) { 147 | System.out.println("Number of mega-nodes that are used for STC: " + megaNodesCount); 148 | System.out.println("Number of sub-nodes that will be used for trajectories: " + 4.0 * megaNodesCount); 149 | } 150 | 151 | } 152 | 153 | private void betterCoverage(double xMin, double yMin, int nodeDistance, boolean hideInfo) { 154 | 155 | megaNodesCount = 0; 156 | megaNodes = new double[xNodes][yNodes][3]; 157 | for (int i = 0; i < xNodes; i++) { 158 | for (int j = 0; j < yNodes; j++) { 159 | megaNodes[i][j][0] = xMin + i * nodeDistance + shiftX; 160 | megaNodes[i][j][1] = yMin + j * nodeDistance + shiftY; 161 | if (InPolygon.check(new double[]{megaNodes[i][j][0], megaNodes[i][j][1]}, polygonCoordinates)) { 162 | megaNodes[i][j][2] = 0; 163 | megaNodesCount++; 164 | } else { 165 | megaNodes[i][j][2] = 1; 166 | } 167 | // // Uncomment to print the mega-nodes 168 | // if (!hideInfo){ 169 | // System.out.println(megaNodes[i][j][0]+", "+megaNodes[i][j][1]+" ---> "+megaNodes[i][j][2]); 170 | // } 171 | } 172 | } 173 | if (!hideInfo) { 174 | System.out.println("Number of mega-nodes inside polygon: " + megaNodesCount); 175 | System.out.println("Number of sub-nodes that will be used for trajectories: " + 4.0 * megaNodesCount); 176 | } 177 | 178 | subNodes = new double[2 * xNodes][2 * yNodes][3]; 179 | for (int i = 0; i < xNodes; i++) { 180 | for (int j = 0; j < yNodes; j++) { 181 | 182 | subNodes[2 * i][2 * j + 1][0] = megaNodes[i][j][0] - nodeIntervalOffset; 183 | subNodes[2 * i][2 * j + 1][1] = megaNodes[i][j][1] + nodeIntervalOffset; 184 | 185 | subNodes[2 * i + 1][2 * j + 1][0] = megaNodes[i][j][0] + nodeIntervalOffset; 186 | subNodes[2 * i + 1][2 * j + 1][1] = megaNodes[i][j][1] + nodeIntervalOffset; 187 | 188 | subNodes[2 * i][2 * j][0] = megaNodes[i][j][0] - nodeIntervalOffset; 189 | subNodes[2 * i][2 * j][1] = megaNodes[i][j][1] - nodeIntervalOffset; 190 | 191 | subNodes[2 * i + 1][2 * j][0] = megaNodes[i][j][0] + nodeIntervalOffset; 192 | subNodes[2 * i + 1][2 * j][1] = megaNodes[i][j][1] - nodeIntervalOffset; 193 | 194 | checkInPolyAndObstacle(i, j); 195 | 196 | subNodes[2 * i][2 * j + 1][2] = megaNodes[i][j][2]; 197 | subNodes[2 * i + 1][2 * j + 1][2] = megaNodes[i][j][2]; 198 | subNodes[2 * i][2 * j][2] = megaNodes[i][j][2]; 199 | subNodes[2 * i + 1][2 * j][2] = megaNodes[i][j][2]; 200 | 201 | // // Uncomment to print all the sub-nodes 202 | // System.out.println(subNodes[2*i+1][2*j][0]+", "+subNodes[2*i][2*j][1]+" ---> "+subNodes[2*i][2*j][2]); 203 | // System.out.println(subNodes[2*i+1][2*j][0]+", "+subNodes[2*i+1][2*j][1]+" ---> "+subNodes[2*i+1][2*j][2]); 204 | // System.out.println(subNodes[2*i][2*j+1][0]+", "+subNodes[2*i][2*j+1][1]+" ---> "+subNodes[2*i][2*j+1][2]); 205 | // System.out.println(subNodes[2*i+1][2*j+1][0]+", "+subNodes[2*i+1][2*j+1][1]+" ---> "+subNodes[2*i+1][2*j+1][2]); 206 | // 207 | // // Uncomment to print the sub-nodes that will be used for trajectories 208 | // if (nodes[i][j][2]!=1){ 209 | // System.out.println(subNodes[2*i][2*j][0]+", "+subNodes[2*i][2*j][1]); 210 | // System.out.println(subNodes[2*i+1][2*j][0]+", "+subNodes[2*i+1][2*j][1]); 211 | // System.out.println(subNodes[2*i][2*j+1][0]+", "+subNodes[2*i][2*j+1][1]); 212 | // System.out.println(subNodes[2*i+1][2*j+1][0]+", "+subNodes[2*i+1][2*j+1][1]); 213 | // } 214 | } 215 | 216 | } 217 | 218 | } 219 | 220 | private void checkInPolyAndObstacle(int i, int j) { 221 | if (cartObst.length > 0){ 222 | for (int k=0; k0) 251 | equalMarginsTerm = marginNormSSI(); 252 | else 253 | equalMarginsTerm = 1; 254 | 255 | optimizationIndex = a*nodesInTerm + b*minBBAreaTerm - c*equalMarginsTerm; 256 | 257 | ConnectComponent G2G = new ConnectComponent(); 258 | int[][] connectivityTest = new int[xNodes][yNodes]; 259 | for (int i = 0; i < xNodes; i++) { 260 | for (int j = 0; j < yNodes; j++) { 261 | connectivityTest[i][j] = (int) Math.abs(megaNodes[i][j][2] - 1); 262 | } 263 | } 264 | G2G.compactLabeling(connectivityTest, new Dimension(yNodes, xNodes), true); 265 | if (G2G.getMaxLabel() > 1) { 266 | optimizationIndex -= .5; 267 | } 268 | 269 | return optimizationIndex; 270 | } 271 | 272 | private double marginNormSSI(){ 273 | 274 | double SSI; 275 | 276 | double[][] coords = new double[4*megaNodesCount][2]; 277 | 278 | int c = 0; 279 | for (int i = 0; i < 2*xNodes; i++) { 280 | for (int j = 0; j < 2*yNodes; j++) { 281 | if (subNodes[i][j][2] != 1){ 282 | coords[c][0] = subNodes[i][j][0]; 283 | coords[c][1] = subNodes[i][j][1]; 284 | c++; 285 | } 286 | } 287 | } 288 | 289 | double xBoxMax = MinMax.xMax(coords); 290 | double xBoxMin = MinMax.xMin(coords); 291 | double yBoxMax = MinMax.yMax(coords); 292 | double yBoxMin = MinMax.yMin(coords); 293 | 294 | SSI = Math.abs(Math.abs(xBoxMax - xMax) - Math.abs(xMin - xBoxMin))/(2*Math.abs(xBoxMax - xBoxMin)) + Math.abs(Math.abs(yBoxMax - yMax) - Math.abs(yMin - yBoxMin))/(2*Math.abs(yBoxMax - yBoxMin)); 295 | 296 | return SSI; 297 | } 298 | 299 | public double[][][] getMegaNodes() { 300 | return megaNodes; 301 | } 302 | 303 | public double[][][] getSubNodes() { 304 | return subNodes; 305 | } 306 | 307 | public int getMegaNodesInCount() { 308 | return megaNodesCount; 309 | } 310 | 311 | public double getBoundingBoxArea() { 312 | return (xRngMeters * yRngMeters); 313 | } 314 | 315 | public double getPolygonArea(){ 316 | 317 | double sum = 0; 318 | for (int i = 0; i < polygonCoordinates.length ; i++) 319 | { 320 | if (i == 0) 321 | { 322 | sum += polygonCoordinates[i][0] * (polygonCoordinates[i + 1][1] - polygonCoordinates[polygonCoordinates.length - 1][1]); 323 | } 324 | else if (i == polygonCoordinates.length - 1) 325 | { 326 | sum += polygonCoordinates[i][0] * (polygonCoordinates[0][1] - polygonCoordinates[i - 1][1]); 327 | } 328 | else 329 | { 330 | sum += polygonCoordinates[i][0] * (polygonCoordinates[i + 1][1] - polygonCoordinates[i - 1][1]); 331 | } 332 | } 333 | 334 | double area = 0.5 * Math.abs(sum); 335 | return area; 336 | 337 | } 338 | 339 | public double getPolygonAreaWithoutObstacles(){ 340 | 341 | double polygonArea = getPolygonArea(); 342 | 343 | double obstaclesArea = 0; 344 | if (cartObst.length > 0) { 345 | for (int k=0; k 7 | * ECEF uses three-dimensional XYZ coordinates (in meters) to describe a location. The term 8 | * "Earth-Centered" comes from the fact that the origin of the axis (0,0,0) is located at the mass 9 | * center of gravity. The term "Earth-Fixed" implies that the axes are fixed with respect to the 10 | * earth. The Z-axis pierces the North Pole, and the XY-axis defines the equatorial plane. 11 | */ 12 | public class ECEF { 13 | /** X coordinate in meter. */ 14 | public double x; 15 | /** Y coordinate in meter. */ 16 | public double y; 17 | /** Z coordinate in meter. */ 18 | public double z; 19 | 20 | /** 21 | * Constructs a point at the origin of the ECEF coordinate system. 22 | */ 23 | public ECEF() { 24 | this.x = 0; 25 | this.y = 0; 26 | this.z = 0; 27 | } 28 | 29 | /** 30 | * Constructs a point at a given ECEF location. 31 | * 32 | * @param x X coordinate in meter. 33 | * @param y Y coordinate in meter. 34 | * @param z Z coordinate in meter. 35 | */ 36 | public ECEF(double x, double y, double z) { 37 | this.x = x; 38 | this.y = y; 39 | this.z = z; 40 | } 41 | } -------------------------------------------------------------------------------- /src/main/java/pathPlanning/handleGeo/coordinates/NED.java: -------------------------------------------------------------------------------- 1 | package pathPlanning.handleGeo.coordinates; 2 | 3 | /** 4 | * Representation of a point in the North/East/Down (NED) geographical coordinate system. 5 | */ 6 | public class NED { 7 | /** North coordinate in meter. */ 8 | public double north; 9 | /** East coordinate in meter. */ 10 | public double east; 11 | /** Down coordinate in meter. */ 12 | public double down; 13 | 14 | /** 15 | * Constructs a point at the origin of the NED coordinate system. 16 | */ 17 | public NED() { 18 | this.north = 0; 19 | this.east = 0; 20 | this.down = 0; 21 | } 22 | 23 | /** 24 | * Constructs a point at a given ECEF location. 25 | * 26 | * @param north North coordinate in meter. 27 | * @param east East coordinate in meter. 28 | * @param down Down coordinate in meter. 29 | */ 30 | public NED(double north, double east, double down) { 31 | this.north = north; 32 | this.east = east; 33 | this.down = down; 34 | } 35 | } -------------------------------------------------------------------------------- /src/main/java/pathPlanning/handleGeo/coordinates/WGS84.java: -------------------------------------------------------------------------------- 1 | package pathPlanning.handleGeo.coordinates; 2 | 3 | /** 4 | * Created by rasm on 8/3/15. 5 | */ 6 | public class WGS84 { 7 | /** Semi-major axis (Equatorial Radius). */ 8 | private static final double SEMI_MAJOR_AXIS = 6378137.0; 9 | /** First eccentricity squared. */ 10 | private static final double FIRST_ECCENTRICITY_SQUARED = 0.00669437999013; 11 | /** Latitude in radian. */ 12 | public double latitude; 13 | /** Longitude in radian. */ 14 | public double longitude; 15 | /** Height in meter. */ 16 | public double height; 17 | 18 | /** 19 | * Construct WGS-84 coordinate with zero values. 20 | */ 21 | public WGS84() { 22 | this.latitude = 0; 23 | this.longitude = 0; 24 | this.height = 0; 25 | } 26 | 27 | /** 28 | * Construct WGS-84 coordinate from given latitude, longitude, and height. 29 | * 30 | * @param latitude latitude in radian. 31 | * @param longitude longitude in radian. 32 | * @param height height in meter. 33 | */ 34 | public WGS84(double latitude, double longitude, double height) { 35 | this.latitude = latitude; 36 | this.longitude = longitude; 37 | this.height = height; 38 | } 39 | 40 | /** 41 | * Computes the distance between two WGS-84 coordinates. 42 | * 43 | * @param pointA first WGS-84 coordinate. 44 | * @param pointB second WGS-84 coordinate. 45 | * @return distance in meter. 46 | */ 47 | public static double distance(WGS84 pointA, WGS84 pointB) { 48 | ECEF ecefA = toECEF(pointA); 49 | ECEF ecefB = toECEF(pointB); 50 | 51 | double x = ecefA.x -= ecefB.x; 52 | double y = ecefA.y -= ecefB.y; 53 | double z = ecefA.z -= ecefB.z; 54 | 55 | return Math.sqrt(x * x + y * y + z * z); 56 | } 57 | 58 | /** 59 | * Compute North-East-Down displacement between two WGS-84 coordinates. 60 | * 61 | * @param pointA WGS-84 coordinates of point A. 62 | * @param pointB WGS-84 coordinates of point B. 63 | * @return displacement in North-East-Down coordinates. 64 | */ 65 | public static NED displacement(WGS84 pointA, WGS84 pointB) { 66 | ECEF ecefA = toECEF(pointA); 67 | ECEF ecefB = toECEF(pointB); 68 | 69 | double ox = ecefB.x - ecefA.x; 70 | double oy = ecefB.y - ecefA.y; 71 | double oz = ecefB.z - ecefA.z; 72 | double slat = Math.sin(pointA.latitude); 73 | double clat = Math.cos(pointA.latitude); 74 | double slon = Math.sin(pointA.longitude); 75 | double clon = Math.cos(pointA.longitude); 76 | 77 | NED ned = new NED(); 78 | ned.north = -slat * clon * ox - slat * slon * oy + clat * oz; 79 | ned.east = -slon * ox + clon * oy; 80 | ned.down = -clat * clon * ox - clat * slon * oy - slat * oz; 81 | 82 | return ned; 83 | } 84 | 85 | /** 86 | * Displace a WGS-84 coordinate in the NED frame according to given offsets. 87 | * 88 | * @param wgs84 reference WGS-84 coordinate. 89 | * @param ned North-East-Down displacement. 90 | * @return displaced WGS-84 coordinate. 91 | */ 92 | public static WGS84 displace(WGS84 wgs84, NED ned) { 93 | ECEF ecef = toECEF(wgs84); 94 | 95 | // Compute Geocentric latitude. 96 | double phi = Math.atan2(ecef.z, Math.sqrt(ecef.x * ecef.x + ecef.y * ecef.y)); 97 | 98 | // Compute all needed sine and cosine terms for conversion. 99 | double slon = Math.sin(wgs84.longitude); 100 | double clon = Math.cos(wgs84.longitude); 101 | double sphi = Math.sin(phi); 102 | double cphi = Math.cos(phi); 103 | 104 | // Obtain ECEF coordinates of displaced point 105 | // Note: some signs from standard ENU formula 106 | // are inverted - we are working with NED (= END) coordinates 107 | ecef.x += -slon * ned.east - clon * sphi * ned.north - clon * cphi * ned.down; 108 | ecef.y += clon * ned.east - slon * sphi * ned.north - slon * cphi * ned.down; 109 | ecef.z += cphi * ned.north - sphi * ned.down; 110 | 111 | // Convert back to WGS-84 coordinates. 112 | return fromECEF(ecef); 113 | } 114 | 115 | /** 116 | * Convert WGS-84 coordinates to ECEF (Earth Center Earth Fixed) coordinates. 117 | * 118 | * @param wgs84 WGS-84 coordinates. 119 | * 120 | * @return corresponding ECEF coordinates. 121 | */ 122 | public static ECEF toECEF(WGS84 wgs84) { 123 | double cos_lat = Math.cos(wgs84.latitude); 124 | double sin_lat = Math.sin(wgs84.latitude); 125 | double cos_lon = Math.cos(wgs84.longitude); 126 | double sin_lon = Math.sin(wgs84.longitude); 127 | double rn = computeRn(wgs84.latitude); 128 | 129 | ECEF ecef = new ECEF(); 130 | ecef.x = (rn + wgs84.height) * cos_lat * cos_lon; 131 | ecef.y = (rn + wgs84.height) * cos_lat * sin_lon; 132 | ecef.z = (((1.0 - FIRST_ECCENTRICITY_SQUARED) * rn) + wgs84.height) * sin_lat; 133 | return ecef; 134 | } 135 | 136 | /** 137 | * Convert ECEF (x,y,z) to WGS-84 (lat, lon, hae). 138 | * 139 | * @param ecef ECEF coordinates. 140 | */ 141 | public static WGS84 fromECEF(ECEF ecef) { 142 | WGS84 wgs84 = new WGS84(); 143 | 144 | double p = Math.sqrt(ecef.x * ecef.x + ecef.y * ecef.y); 145 | 146 | wgs84.longitude = Math.atan2(ecef.y, ecef.x); 147 | wgs84.latitude = Math.atan2(ecef.z / p, 0.01); 148 | double n = computeRn(wgs84.latitude); 149 | wgs84.height = p / Math.cos(wgs84.latitude) - n; 150 | double old_hae = -1e-9; 151 | double num = ecef.z / p; 152 | 153 | while (Math.abs(wgs84.height - old_hae) > 1e-4) { 154 | old_hae = wgs84.height; 155 | double den = 1 - FIRST_ECCENTRICITY_SQUARED * n / (n + wgs84.height); 156 | wgs84.latitude = Math.atan2(num, den); 157 | n = computeRn(wgs84.latitude); 158 | wgs84.height = p / Math.cos(wgs84.latitude) - n; 159 | } 160 | 161 | return wgs84; 162 | } 163 | 164 | private static double computeRn(double lat) { 165 | double lat_sin = Math.sin(lat); 166 | return SEMI_MAJOR_AXIS / Math.sqrt(1 - FIRST_ECCENTRICITY_SQUARED * (lat_sin * lat_sin)); 167 | } 168 | 169 | @Override 170 | public String toString() { 171 | return String.format("%.6f, %.6f, %.2f", Math.toDegrees(latitude), Math.toDegrees(longitude), height); 172 | } 173 | } -------------------------------------------------------------------------------- /src/main/java/pathPlanning/helpers/MinMax.java: -------------------------------------------------------------------------------- 1 | package pathPlanning.helpers; 2 | 3 | public class MinMax { 4 | 5 | public static int indMaxNodeMinArea(int[] nodesIn, double[] area) { 6 | if (nodesIn.length != area.length || nodesIn.length == 0 || area.length == 0) 7 | return -1; // null/empty or unequal length of matrices 8 | 9 | int returnIND = 0; 10 | int largest = -Integer.MAX_VALUE; 11 | double bestArea = Double.MAX_VALUE; 12 | for (int i = 0; i < nodesIn.length; i++) { 13 | if (nodesIn[i] > largest) { 14 | largest = nodesIn[i]; 15 | bestArea = area[i]; 16 | returnIND = i; 17 | } else if (nodesIn[i] == largest && area[i] < bestArea) { 18 | bestArea = area[i]; 19 | returnIND = i; 20 | } 21 | } 22 | 23 | return returnIND; 24 | } 25 | 26 | public static int indMax(int[] array) { 27 | 28 | if (array == null || array.length == 0) return -1; // null or empty 29 | 30 | int largest = 0; 31 | for (int i = 1; i < array.length; i++) { 32 | if (array[i] > array[largest]) largest = i; 33 | } 34 | 35 | return largest; 36 | } 37 | 38 | public static int indMin(double[] array) { 39 | 40 | if (array == null || array.length == 0) return -1; // null or empty 41 | 42 | int smallest = 0; 43 | for (int i = 1; i < array.length; i++) { 44 | if (array[i] < array[smallest]) { 45 | smallest = i; 46 | } 47 | } 48 | 49 | return smallest; 50 | } 51 | 52 | public static double xMax(double[][] polygonCoordinates) { 53 | int l = polygonCoordinates.length; 54 | double xMax = -Double.MAX_VALUE; 55 | 56 | for (int i = 0; i < l; i++) { 57 | if (polygonCoordinates[i][0] > xMax) { 58 | xMax = polygonCoordinates[i][0]; 59 | } 60 | } 61 | 62 | return xMax; 63 | } 64 | 65 | public static double yMax(double[][] polygonCoordinates) { 66 | int l = polygonCoordinates.length; 67 | double yMax = -Double.MAX_VALUE; 68 | 69 | for (int i = 0; i < l; i++) { 70 | if (polygonCoordinates[i][1] > yMax) { 71 | yMax = polygonCoordinates[i][1]; 72 | } 73 | } 74 | 75 | return yMax; 76 | } 77 | 78 | public static double xMin(double[][] polygonCoordinates) { 79 | int l = polygonCoordinates.length; 80 | double xMin = Double.MAX_VALUE; 81 | 82 | for (int i = 0; i < l; i++) { 83 | if (polygonCoordinates[i][0] < xMin) { 84 | xMin = polygonCoordinates[i][0]; 85 | } 86 | } 87 | 88 | return xMin; 89 | } 90 | 91 | public static double yMin(double[][] polygonCoordinates) { 92 | int l = polygonCoordinates.length; 93 | double yMin = Double.MAX_VALUE; 94 | 95 | for (int i = 0; i < l; i++) { 96 | if (polygonCoordinates[i][1] < yMin) { 97 | yMin = polygonCoordinates[i][1]; 98 | } 99 | } 100 | 101 | return yMin; 102 | } 103 | 104 | } 105 | -------------------------------------------------------------------------------- /src/main/java/pathPlanning/nodesPlacementOptimization/Rotate.java: -------------------------------------------------------------------------------- 1 | package pathPlanning.nodesPlacementOptimization; 2 | 3 | import pathPlanning.handleGeo.NodesInPoly; 4 | import pathPlanning.helpers.MinMax; 5 | 6 | import java.util.ArrayList; 7 | 8 | public class Rotate { 9 | 10 | private float optimalTheta = 0.0f; 11 | private int range = 90; 12 | 13 | public void setParameters(float optimalTheta, int range) { 14 | this.optimalTheta = optimalTheta; 15 | this.range = range; 16 | } 17 | 18 | public void setTheta(float theta){ 19 | optimalTheta = theta; 20 | } 21 | 22 | public void setRange(int range){ 23 | this.range = range; 24 | } 25 | 26 | public void findOptimalTheta(int scanDist, double[][] cartUnrotated, double[][][] cartObstUnrotated, float stepSize, double shiftX, double shiftY){ 27 | int testRange = (int) (range/stepSize); 28 | 29 | float theta; 30 | int[] nodesIn = new int[testRange]; 31 | double[] area = new double[testRange]; 32 | 33 | optimalTheta = 0.0f; 34 | for (int i=0; i rotateBackWaypoints(ArrayList iWaypoints){ 68 | 69 | float minusTheta = -optimalTheta; 70 | 71 | int l = iWaypoints.size(); 72 | ArrayList waypoints = new ArrayList<>(); 73 | 74 | for (int i=0; i Tmin) { 49 | 50 | for (int i=0; i optimizationIndexMax){ 55 | optimizationIndexMax = optimizationIndexCurrent; 56 | optimalTheta = currentSol.getTheta(); 57 | optimalShiftX = currentSol.getShiftX(); 58 | optimalShiftY = currentSol.getShiftY(); 59 | } 60 | 61 | Solution newSol = new Solution(); 62 | newSol.Random(); 63 | // if (i == numIterations-1){ 64 | // newSol.Random(); 65 | // } else { 66 | // newSol.Neighbor(); 67 | // } 68 | newSol.CreateNew(); 69 | double ap = Math.pow(Math.E, (optimizationIndexCurrent - newSol.getOptimizationIndex())/T); 70 | if (ap > Math.random()) 71 | currentSol = newSol; 72 | } 73 | 74 | T *= alpha; // Decreases T, cooling phase 75 | } 76 | System.out.println("\n ~~~ Final value of optimization index: "+optimizationIndexMax+" ~~~ "); 77 | 78 | } 79 | 80 | private class Solution{ 81 | 82 | private float theta; 83 | private float shiftX; 84 | private float shiftY; 85 | private double optimizationIndex; 86 | 87 | public void Initialization(){ 88 | theta = 0; 89 | shiftX = scanDist; 90 | shiftY = scanDist; 91 | } 92 | 93 | public void Random(){ 94 | theta = (float) Math.random() * 90; 95 | shiftX = (float) Math.random() * 2 * scanDist; 96 | shiftY = (float) Math.random() * 2 * scanDist; 97 | } 98 | 99 | public void Neighbor(){ 100 | 101 | int a; 102 | int b; 103 | int c; 104 | 105 | if (Math.random() > .5) a = 45; 106 | else a = -45; 107 | if (Math.random() > .5) b = scanDist/4; 108 | else b = -scanDist/4; 109 | if (Math.random() > .5) c = scanDist/4; 110 | else c = -scanDist/4; 111 | 112 | theta = theta + a*((float) Math.random()); 113 | shiftX = shiftX + b*((float) Math.random()); 114 | shiftY = shiftY + c*((float) Math.random()); 115 | 116 | } 117 | 118 | public void CreateNew(){ 119 | 120 | Transformations randomSol = new Transformations(); 121 | randomSol.setTheta(theta); 122 | randomSol.setShiftX(shiftX); 123 | randomSol.setShiftY(shiftY); 124 | optimizationIndex = randomSol.rotateAndShift(cart, cartObst, scanDist); 125 | 126 | } 127 | 128 | public float getTheta() { 129 | return theta; 130 | } 131 | 132 | public float getShiftX() { 133 | return shiftX; 134 | } 135 | 136 | public float getShiftY() { 137 | return shiftY; 138 | } 139 | 140 | public double getOptimizationIndex() { 141 | return optimizationIndex; 142 | } 143 | 144 | } 145 | 146 | 147 | } 148 | -------------------------------------------------------------------------------- /src/main/java/pathPlanning/nodesPlacementOptimization/Transformations.java: -------------------------------------------------------------------------------- 1 | package pathPlanning.nodesPlacementOptimization; 2 | 3 | import pathPlanning.handleGeo.NodesInPoly; 4 | 5 | public class Transformations { 6 | 7 | float theta; 8 | float shiftX; 9 | float shiftY; 10 | 11 | public float getTheta() { 12 | return theta; 13 | } 14 | public float getShiftX() { 15 | return shiftX; 16 | } 17 | public float getShiftY() { 18 | return shiftY; 19 | } 20 | 21 | public void setTheta(float theta) { 22 | this.theta = theta; 23 | } 24 | public void setShiftX(float shiftX) { 25 | this.shiftX = shiftX; 26 | } 27 | public void setShiftY(float shiftY) { 28 | this.shiftY = shiftY; 29 | } 30 | 31 | public double rotateAndShift(double[][] cart, double[][][] cartObst, int scanDist){ 32 | 33 | double[][] rotated; 34 | double[][][] rotatedObst = new double[][][]{}; 35 | 36 | Rotate rotate = new Rotate(); 37 | rotate.setTheta(theta); 38 | rotated = rotate.rotatePolygon(cart); 39 | 40 | if (cartObst.length > 0) { 41 | rotatedObst = new double[cartObst.length][][]; 42 | for (int i=0; i 2 | 3 | 4 | 5 | 6 | 7 | Polygon on map 8 | 36 | 37 | 38 | 39 |
40 |
41 | 42 | 43 |
44 | 45 | 117 | 120 | 121 | -------------------------------------------------------------------------------- /src/main/resources/visualizeWGS84.m: -------------------------------------------------------------------------------- 1 | clear; 2 | clc; 3 | close all; 4 | 5 | 6 | 7 | %% Polygon ROI - coverage paths - coverage distance 8 | 9 | polygonWGS84 = [40.873884282708275, 24.312673846131773; 10 | 40.873728114416515, 24.31273330177858; 11 | 40.87360034008555, 24.3129215034581; 12 | 40.873545579582554, 24.313190171408067; 13 | 40.873555720419844, 24.313501754702273; 14 | 40.87359222742132, 24.313648158617823; 15 | 40.873661185035814, 24.313783833697315; 16 | 40.873758536839944, 24.313891904372284; 17 | 40.87386400113278, 24.313978517375133; 18 | 40.87399785941641, 24.31410804572222; 19 | 40.8740647884567, 24.314237182912123; 20 | 40.87409622480081, 24.314341984642297; 21 | 40.87411143592965, 24.31448970171671; 22 | 40.8739248458407, 24.316089192386542; 23 | 40.875318779965916, 24.316346684451972; 24 | 40.87574062618736, 24.312577286607237; 25 | 40.87517412533149, 24.312598744279356; 26 | 40.8749767391839, 24.312829414254637; 27 | 40.87408775656638, 24.312657752877683]; 28 | 29 | obstacles = {[40.87435937003009, 24.313325920763702; 30 | 40.87430866644355, 24.31344781227139; 31 | 40.87428230056317, 24.313612619123276; 32 | 40.874245793942215, 24.31376267382558; 33 | 40.87410787985927, 24.314191678265473; 34 | 40.87490857822834, 24.31438747952356; 35 | 40.87499375945969, 24.31393180199605; 36 | 40.874996167856025, 24.3138444811903; 37 | 40.874974238770626, 24.313762524802627; 38 | 40.87495332374008, 24.313698002773506; 39 | 40.87492023995101, 24.31362006969931; 40 | 40.87490071924024, 24.31354601092592; 41 | 40.87489742353518, 24.313482680988585; 42 | 40.87491516963704, 24.313404300876144; 43 | 40.87494102823421, 24.313331285181732; 44 | 40.874623834026536, 24.31324813670227; 45 | 40.874463208213406, 24.31324411338875]}; 46 | 47 | path = [40.87511606353929, 24.31620760355072; 48 | 40.8752938771259, 24.31624525463758; 49 | 40.875722508208646, 24.312731338258402; 50 | 40.87518906400966, 24.31261841297073; 51 | 40.875131919836605, 24.313086934114185; 52 | 40.87370940222757, 24.312785799110443; 53 | 40.87362368419745, 24.31348856472302; 54 | 40.87380149874874, 24.31352620772088; 55 | 40.873858644728095, 24.313057696296713; 56 | 40.874036459407904, 24.313095338278785; 57 | 40.873922165245325, 24.314032362757327; 58 | 40.87409997960375, 24.31407000737295; 59 | 40.87421427406625, 24.313132980461504; 60 | 40.875103347036074, 24.313321194384923; 61 | 40.874960475893324, 24.31449249272835; 62 | 40.87424921906012, 24.31434190902361; 63 | 40.87404918156848, 24.315981701244166; 64 | 40.87422699528397, 24.31601935112686; 65 | 40.87439845786906, 24.31461381189068; 66 | 40.87457627201311, 24.314651458324757; 67 | 40.874404808977985, 24.316057001210247; 68 | 40.87458262265053, 24.31609465149432; 69 | 40.87475408613569, 24.31468910495951; 70 | 40.87493190023679, 24.314726751794936; 71 | 40.8747604363016, 24.31613230197909; 72 | 40.87493824993119, 24.31616995266456; 73 | 40.875338306840646, 24.312890315812947; 74 | 40.87551612149866, 24.312927958183753; 75 | 40.87511606353929, 24.31620760355072]; 76 | 77 | 78 | 79 | %% Transform to NED 80 | % Transformation for polygon 81 | [n,m] = size(polygonWGS84); 82 | polygonNED = zeros(n,m); 83 | wgs84 = wgs84Ellipsoid; 84 | fprintf(' ~~~ Polygon hull: ~~~ \n\n'); 85 | for i=1:n 86 | [polygonNED(i,1), polygonNED(i,2)] = geodetic2ned(polygonWGS84(i,1),polygonWGS84(i,2),0,polygonWGS84(1,1),polygonWGS84(1,2),0,wgs84); 87 | fprintf('- {x: %f, y: %f, z: %f}\n', polygonNED(i,2), polygonNED(i,1), 4.0); 88 | end 89 | 90 | % Transformation for obstacles 91 | if (~isempty(obstacles)) 92 | fprintf('\n\n ~~~ Obstacles: ~~~ \n'); 93 | for i = 1:length(obstacles) 94 | fprintf('\n - NFZ %d: \n',i); 95 | [o1,o2] = size(obstacles{i}); 96 | obNED = zeros(o1,o2); 97 | for j=1:o1 98 | [obNED(j,1), obNED(j,2)] = geodetic2ned(obstacles{i}(j,1),obstacles{i}(j,2),0,polygonWGS84(1,1),polygonWGS84(1,2),0,wgs84); 99 | fprintf('- {x: %f, y: %f, z: %f}\n', obNED(j,2), obNED(j,1), 4.0); 100 | end 101 | obstNED{i} = obNED; 102 | end 103 | end 104 | 105 | % Transformation for path 106 | [k,l] = size(path); 107 | pathNED = zeros(k,l); 108 | for i=1:k 109 | [pathNED(i,1), pathNED(i,2)] = geodetic2ned(path(i,1),path(i,2),0,polygonWGS84(1,1),polygonWGS84(1,2),0,wgs84); 110 | end 111 | 112 | %% Plot paths in poly 113 | figure; 114 | 115 | hold on; 116 | 117 | title('Polygon ROI with paths') 118 | axis equal; 119 | axis off; 120 | 121 | fill(polygonNED(:,2),polygonNED(:,1),'g'); % polygon 122 | 123 | if (~isempty(obstacles)) % obstacles 124 | for i=1:length(obstacles) 125 | fill(obstNED{i}(:,2), obstNED{i}(:,1),'r') 126 | end 127 | end 128 | 129 | plot(pathNED(:,2),pathNED(:,1),'k --'); % paths 130 | 131 | hold off; 132 | -------------------------------------------------------------------------------- /src/test/java/DARPTester.java: -------------------------------------------------------------------------------- 1 | import pathPlanning.darp.ConnectComponent; 2 | import pathPlanning.darp.DARP; 3 | 4 | import java.awt.*; 5 | 6 | 7 | public class DARPTester { 8 | 9 | public static void main (String[] args) { 10 | // ------------------------------------------------------------------------------------------------------------- 11 | // DARP parameters 12 | // ------------------------------------------------------------------------------------------------------------- 13 | int droneNo = 4; // #UAVs 14 | boolean notEqualPortions = false; // # For proportional area allocation --> true 15 | double[] Rportions = new double[]{}; // When proportional area allocation is asked, provide portions 16 | 17 | 18 | // Grid size 19 | int l = 20; 20 | int m = 20; 21 | 22 | 23 | // Grid that will be given as input to DARP 24 | int[][] DARPgrid; 25 | 26 | //---------------------------------------------- 27 | // Initianize GRID (or give grid as input) 28 | //---------------------------------------------- 29 | // In DARPgrid 0 stands for free space 30 | // 1 stands for Obstacle 31 | // 2 stands for Drone 32 | DARPgrid = new int[l][m]; 33 | // Grid representation 34 | for (int i = 0; i < l; i++) { 35 | for (int j = 0; j < m; j++) { 36 | DARPgrid[i][j] = 0; 37 | } 38 | } 39 | //---------------------------------------------- 40 | 41 | 42 | // Check for grid connectivity 43 | ConnectComponent G2G = new ConnectComponent(); 44 | int[][] connectivityTest = new int[l][m]; 45 | for (int i = 0; i < l; i++) { 46 | for (int j = 0; j < m; j++) { 47 | connectivityTest[i][j] = Math.abs(DARPgrid[i][j] - 1); 48 | } 49 | } 50 | G2G.compactLabeling(connectivityTest, new Dimension(m, l), true); 51 | if (G2G.getMaxLabel() > 1) { 52 | System.out.println("\n\n !!! The environment grid MUST not have" + 53 | " unreachable and/or closed shape regions !!! \n\n"); 54 | return; 55 | } 56 | 57 | 58 | //initial positions of drone(s) 59 | DARPgrid[0][1] = 2; 60 | DARPgrid[2][10] = 2; 61 | DARPgrid[3][16] = 2; 62 | DARPgrid[4][3] = 2; 63 | 64 | 65 | // Parameters to run DARP 66 | int MaxIter = 80000; 67 | double CCvariation = 0.01; 68 | double randomLevel = 0.0001; 69 | int dcells = 2; 70 | boolean importance = false; 71 | 72 | 73 | // If user has not defined custom portions divide area equally for all drones 74 | if (!notEqualPortions) { 75 | Rportions = new double[droneNo]; 76 | for (int i = 0; i < droneNo; i++) { 77 | Rportions[i] = 1.0/droneNo; 78 | } 79 | } 80 | 81 | System.out.println("Portion of the total area assigned to each drone:"); 82 | for (int i = 0; i < droneNo; i++) { 83 | System.out.println(" - "+Rportions[i]*100+" %"); 84 | } 85 | System.out.println(); 86 | 87 | // Perform operational area division (run DARP) 88 | DARP problem = new DARP(l, m, DARPgrid, MaxIter, CCvariation, randomLevel, dcells, importance, Rportions); 89 | 90 | // Warn when no drone is defined 91 | System.out.println("Number of drones: " + problem.getNr()); 92 | if (problem.getNr() <= 0) { 93 | System.out.println("\n\n !!! You should use at least one drone !!!\n"); 94 | return; 95 | } 96 | 97 | // Run DARP 98 | int[][] DARPAssignmentMatrix = new int[0][]; 99 | problem.constructAssignmentM(); 100 | if (problem.getSuccess()) { 101 | DARPAssignmentMatrix = problem.getAssignmentMatrix(); 102 | } else { 103 | System.out.println("\nDARP did not manage to find a solution for the given configuration!"); 104 | return; 105 | } 106 | 107 | System.out.println("\nTotal iterations needed: "+problem.getIter()); 108 | System.out.println("\nAssignment matrix: "); 109 | for (int i=0; i paths strictly in polygon mode/false --> better coverage mode 38 | 39 | double[][] initialPos = new double[][] // initial positions of the vehicles (WGS84 coordinates) 40 | {{40.93267,24.40603}, {40.93131,24.41342}, {40.93534,24.41271}}; 41 | // double[][] initialPos = new double[][]{}; 42 | 43 | double[] rPortions = new double[]{.3, .6, .1 }; // percentage of the region that each vehicle should undertake (sum should always be 1) 44 | // double[] rPortions = new double[]{}; 45 | //-------------------------------------------------------------------------------------------------------------- 46 | 47 | 48 | 49 | //-------------------------------------------------------------------------------------------------------------- 50 | // Run optimized DARP mCPP algorithm 51 | //-------------------------------------------------------------------------------------------------------------- 52 | DARPinPoly mission = new DARPinPoly(); 53 | if(initialPos.length > 0){ 54 | if (rPortions.length > 0){ 55 | mission.waypointsDARP(droneNo, scanningDensity, polygon, obstacles, pathsStrictlyInPoly, initialPos, rPortions); 56 | } else { 57 | mission.waypointsDARP(droneNo, scanningDensity, polygon, obstacles, pathsStrictlyInPoly, initialPos); 58 | } 59 | } else { 60 | if (rPortions.length > 0){ 61 | mission.waypointsDARP(droneNo, scanningDensity, polygon, obstacles, pathsStrictlyInPoly, rPortions); 62 | } else { 63 | mission.waypointsDARP(droneNo, scanningDensity, polygon, obstacles, pathsStrictlyInPoly); 64 | } 65 | } 66 | mission.printWaypointsWGS84(); 67 | 68 | ArrayList> missionWaypoints = mission.getMissionWaypointsWGS84(); 69 | //-------------------------------------------------------------------------------------------------------------- 70 | 71 | } 72 | 73 | } 74 | --------------------------------------------------------------------------------