├── LICESE.txt
├── RCPP.m
├── README.md
├── adjustDx.m
├── angleAP.m
├── antipodalPoints.m
├── bestPathForAntipodalPair.m
├── data
    ├── p_e_rcpp.txt
    ├── p_x_rcpp.txt
    └── target_rcpp_room.txt
├── distPoint2Line.m
├── getConvexPolygon.m
├── getPathMR.m
├── getPathMR2.m
├── getPolygon.m
├── images
    └── path_example.png
├── isAConvexPolygon.m
├── lineSegmentIntersect.m
├── loadPolygonCSV.m
├── rotatePolygon.m
├── testOptimalCoverageRotCalip.m
├── testRCPP.m
├── timeCost2D.m
├── torres16.m
└── torres16simple.m


/LICESE.txt:
--------------------------------------------------------------------------------
 1 | New BSD License. 
 2 | 
 3 | Copyright (c) 2009-2013, 2017 J. Irving Vasquez-Gomez
 4 | All rights reserved.
 5 | 
 6 | Redistribution and use in source and binary forms, with or without
 7 | modification, are permitted provided that the following conditions are met:
 8 | 
 9 |     * Redistributions of source code must retain the above copyright
10 |       notice, this list of conditions and the following disclaimer.
11 |     * Redistributions in binary form must reproduce the above copyright
12 |       notice, this list of conditions and the following disclaimer in the
13 |       documentation and/or other materials provided with the distribution.
14 |     * Neither the name of the University of Freiburg nor the names of its
15 |       contributors may be used to endorse or promote products derived from
16 |       this software without specific prior written permission.
17 | 
18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
21 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
22 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
23 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
24 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
25 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
26 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
27 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
28 | POSSIBILITY OF SUCH DAMAGE.
29 | 


--------------------------------------------------------------------------------
/RCPP.m:
--------------------------------------------------------------------------------
 1 | % CONACYT
 2 | % Copyright (c) 2017, J. Irving Vasquez-Gomez,
 3 | % All rights reserved.
 4 | % 
 5 | % Redistribution and use in source and binary forms, with or without
 6 | % modification, are permitted provided that the following conditions are met:
 7 | %     * Redistributions of source code must retain the above copyright
 8 | %       notice, this list of conditions and the following disclaimer.
 9 | %     * Redistributions in binary form must reproduce the above copyright
10 | %       notice, this list of conditions and the following disclaimer in the
11 | %       documentation and/or other materials provided with the distribution.
12 | %     * Neither the name of the <organization> nor the
13 | %       names of its contributors may be used to endorse or promote products
14 | %       derived from this software without specific prior written permission.
15 | % 
16 | % THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 | % ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 | % WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 | % DISCLAIMED. IN NO EVENT SHALL J. Irving Vasquez-Gomez BE LIABLE FOR ANY
20 | % DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 | % (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 | % LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 | % ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 | % (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 | % SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 | 
27 | function [optimal_path, min_cost] = RCPP(M, dx, starting_point, ending_point, translation_speed, rot_spd)
28 | % Rotating calipers path planner 
29 | % 
30 | % M : Polygon
31 | % dx distance between flight lines
32 | % translation_speed and rot_spd are used to estimate the cost of a path.
33 | % The speed affects the behaviour of the planner.
34 | 
35 | A = antipodalPoints(M);
36 | [m, ~] = size(A);
37 | 
38 | x_start = starting_point(1);
39 | y_start = starting_point(2);
40 | x_end = ending_point(1);
41 | y_end = ending_point(2);
42 | 
43 | min_cost = Inf;
44 | optimal_path = [];
45 | best_antipodal_pair = 0;
46 | 
47 | for i=1:m 
48 |     Path = bestPathForAntipodalPair(M, A(i,:), dx);
49 |          
50 |     %check if the path should be inverted
51 |     FullPath1 = [x_start y_start; Path;  x_end y_end];
52 |     FullPath2 = [x_start y_start; flipud(Path); x_end y_end];
53 |     
54 |     cost1 = timeCost2D(FullPath1, translation_speed, rot_spd, [x_start y_start 0]);
55 |     
56 |     cost2 =  timeCost2D(FullPath2, translation_speed, rot_spd, [x_start y_start 0]);
57 |     
58 |     if (cost1 < cost2)
59 |        FullPath = FullPath1;
60 |        Cost(i) = cost1;
61 |     else
62 |        FullPath = FullPath2;
63 |        Cost(i) = cost2;
64 |     end
65 |     
66 |     if Cost(i)<min_cost
67 |         min_cost = Cost(i);
68 |         optimal_path = FullPath;
69 |         best_antipodal_pair = i;
70 |     end
71 | end
72 | 
73 | end
74 | 
75 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # Implementation of an Optimal Coverage Path Planner
 2 | 
 3 | Copyright (c) 2017, Juan Irving Vasquez-Gomez, BSD Licese
 4 | Made with support of: Consejo Nacional de Ciencia y Tecnología (CONACYT) and Instituto Politécnico Nacional (IPN), Project cátedra 1507.
 5 | 
 6 | Field surveying by drones has become a successful activity with applications to mapping, precision agriculture, surveillance, etc. The current state of the art path planners for surveying rely on computing the path that minimizes the number of flight lines. However, such approach could be sub-optimal when the takeoff and landing points are included in the path. We propose a method that computes the optimal path, including the takeoff and landing points. Our method is based on the rotating calipers algorithm and it has a *O(n)* complexity.
 7 | 
 8 | Path example:
 9 | ![Path Example](images/path_example.png)
10 | 
11 | The algorithm as been coded on matlab. In addition, the Torres's algorithm has been also implemented for comparison purposes. The code was tested for the last time on MatLab2019b.
12 | 
13 | The planner was early published as a conference paper:
14 | 
15 | > Vasquez-Gomez, J. I., Melchor, M. M., & Lozada, J. C. H. (2017, November). Optimal Coverage Path Planning Based on the Rotating Calipers Algorithm. In Mechatronics, Electronics and Automotive Engineering (ICMEAE), 2017 International Conference on (pp. 140-144). IEEE.
16 | 
17 | Later, a deeper study of the algorithm was done in our Journal paper, where we call it "the rotating calipers path planner" (RCPP):
18 | 
19 | > Juan Irving Vasquez-Gomez, Magdalena Marciano-Melchor, Luis Valentin, Juan Carlos Herrera-Lozada, Coverage Path Planning for 2D Convex Regions. Journal of Intelligent & Robotic Systems. 2019
20 | 
21 | If you are using this code in an academic work, please cite our papers. Paper preprints are available in my [web page][jivg]. If your are using this code in a commercial application please let us know, keep track on our developments and their use help us to keep releasing code.
22 | 
23 | Irving Vasquez, 
24 | 
25 | [jivg.org][jivg]
26 | 
27 | ## Short Usage Tutorial
28 | 
29 | ### Polygons 
30 | 
31 | The polygons are specified as a M x 2 matrix. Where M is the amount of vertices. Ranndom polygons cab ne created with the function getConvexPolygon. A polygon can also be read from a CSV file with *loadPolygonCSV* function.
32 | 
33 | ### Testing RCPP
34 | 
35 | The easiest way is to read or create a polygon and call the RCPP function. See the testRCPP file.
36 | 
37 | [jivg]: https://jivg.org/
38 | 


--------------------------------------------------------------------------------
/adjustDx.m:
--------------------------------------------------------------------------------
 1 | % CONACYT
 2 | % Copyright (c) 2017, Juan Irving Vasquez-Gomez,
 3 | % All rights reserved.
 4 | % 
 5 | % Redistribution and use in source and binary forms, with or without
 6 | % modification, are permitted provided that the following conditions are met:
 7 | %     * Redistributions of source code must retain the above copyright
 8 | %       notice, this list of conditions and the following disclaimer.
 9 | %     * Redistributions in binary form must reproduce the above copyright
10 | %       notice, this list of conditions and the following disclaimer in the
11 | %       documentation and/or other materials provided with the distribution.
12 | %     * Neither the name of the <organization> nor the
13 | %       names of its contributors may be used to endorse or promote products
14 | %       derived from this software without specific prior written permission.
15 | % 
16 | % THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 | % ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 | % WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 | % DISCLAIMED. IN NO EVENT SHALL J. Irving Vasquez-Gomez BE LIABLE FOR ANY
20 | % DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 | % (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 | % LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 | % ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 | % (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 | % SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 | 
27 | function [dx_opt] = adjustDx(dx, h)
28 | nl = ceil(h/dx);
29 | dx_opt = h/nl;
30 | end


--------------------------------------------------------------------------------
/angleAP.m:
--------------------------------------------------------------------------------
 1 | % CONACYT
 2 | % Copyright (c) 2017, Juan Irving Vasquez-Gomez,
 3 | % All rights reserved.
 4 | % 
 5 | % Redistribution and use in source and binary forms, with or without
 6 | % modification, are permitted provided that the following conditions are met:
 7 | %     * Redistributions of source code must retain the above copyright
 8 | %       notice, this list of conditions and the following disclaimer.
 9 | %     * Redistributions in binary form must reproduce the above copyright
10 | %       notice, this list of conditions and the following disclaimer in the
11 | %       documentation and/or other materials provided with the distribution.
12 | %     * Neither the name of the <organization> nor the
13 | %       names of its contributors may be used to endorse or promote products
14 | %       derived from this software without specific prior written permission.
15 | % 
16 | % THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 | % ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 | % WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 | % DISCLAIMED. IN NO EVENT SHALL J. Irving Vasquez-Gomez BE LIABLE FOR ANY
20 | % DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 | % (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 | % LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 | % ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 | % (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 | % SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 | 
27 | function angle = angleAP(P, i, j)
28 |     [n, ~]= size(P);
29 |     
30 |     i_next = mod(i,n)+1; 
31 |     y = P(i_next,2) - P(i,2);
32 |     x = P(i_next,1) - P(i,1);
33 |     alpha_i = atan2(y,x);
34 |     
35 |     j_next = mod(j,n)+1;
36 |     y = P(j_next,2) - P(j,2);
37 |     x = P(j_next,1) - P(j,1);
38 |     alpha_j = atan2(y,x);
39 | 
40 |     angle = clockWiseDist(alpha_i, alpha_j);
41 | 
42 | end
43 | 
44 | % clock wise distance from a to b
45 | function angle = clockWiseDist(a , b)
46 |    if a<0
47 |        if b<0
48 |            if(a<b)
49 |                angle = 2*pi + (a-b);
50 |            else
51 |                angle = a - b;
52 |            end
53 |        else
54 |            a = 2*pi + a;
55 |            angle = a-b;
56 |        end
57 |    else
58 |        if b<0
59 |            angle = a - b;
60 |        else
61 |            if(a<b)
62 |                angle = 2*pi-(a-b);
63 |            else
64 |                angle = a-b;
65 |            end
66 |        end
67 |    end
68 | end


--------------------------------------------------------------------------------
/antipodalPoints.m:
--------------------------------------------------------------------------------
 1 | % CONACYT
 2 | % Copyright (c) 2017, Juan Irving Vasquez-Gomez,
 3 | % All rights reserved.
 4 | % 
 5 | % Redistribution and use in source and binary forms, with or without
 6 | % modification, are permitted provided that the following conditions are met:
 7 | %     * Redistributions of source code must retain the above copyright
 8 | %       notice, this list of conditions and the following disclaimer.
 9 | %     * Redistributions in binary form must reproduce the above copyright
10 | %       notice, this list of conditions and the following disclaimer in the
11 | %       documentation and/or other materials provided with the distribution.
12 | %     * Neither the name of the <organization> nor the
13 | %       names of its contributors may be used to endorse or promote products
14 | %       derived from this software without specific prior written permission.
15 | % 
16 | % THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 | % ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 | % WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 | % DISCLAIMED. IN NO EVENT SHALL J. Irving Vasquez-Gomez BE LIABLE FOR ANY
20 | % DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 | % (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 | % LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 | % ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 | % (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 | % SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 | 
27 | % Computes all antipodal pairs
28 | % Receives a poygon in P, the polygon must be clock wise ordered 
29 | % [px0 py0]
30 | % .
31 | % [pxn pyn]
32 | 
33 | function A = antipodalPoints(P)
34 |     % Find an initial antipodal pair by locating the vertex opposite p1
35 |     [n, ~] = size(P);
36 |     i = 1;
37 |     j = 2;
38 |     A =[];
39 | 
40 |     while angleAP(P, n, j) < pi
41 |         j = increment(j,n);
42 |     end
43 |     A = [A; i j];
44 | 
45 |     while j<=n
46 |         %if (angleAP(P, i, j) <= angleAP(P, j, i))
47 |         diff = pi-angleAP(P,i,j);
48 |         if (diff>0)
49 |             j = j+1;
50 |         else
51 |             if (diff<0)
52 |                 i = i+1;
53 |             else
54 |                 if(i+1<j)
55 |                     A = [A;increment(i,n) j];
56 |                 end
57 |                 if(j+1<=n)
58 |                     A = [A;i increment(j,n)];
59 |                 end                
60 |                 i = i+1;
61 |                 j = j+1;
62 |             end
63 |         end
64 |         if(j<=n && i<j)
65 |             A = [A; i j];
66 |         end
67 |     end
68 | end
69 | 
70 | function i_next = increment(i,n)
71 |     i_next = mod(i,n)+1;
72 | end
73 | 
74 | 


--------------------------------------------------------------------------------
/bestPathForAntipodalPair.m:
--------------------------------------------------------------------------------
  1 | % CONACYT
  2 | % Copyright (c) 2017, Juan Irving Vasquez-Gomez,
  3 | % All rights reserved.
  4 | % 
  5 | % Redistribution and use in source and binary forms, with or without
  6 | % modification, are permitted provided that the following conditions are met:
  7 | %     * Redistributions of source code must retain the above copyright
  8 | %       notice, this list of conditions and the following disclaimer.
  9 | %     * Redistributions in binary form must reproduce the above copyright
 10 | %       notice, this list of conditions and the following disclaimer in the
 11 | %       documentation and/or other materials provided with the distribution.
 12 | %     * Neither the name of the <organization> nor the
 13 | %       names of its contributors may be used to endorse or promote products
 14 | %       derived from this software without specific prior written permission.
 15 | % 
 16 | % THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 17 | % ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 18 | % WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 19 | % DISCLAIMED. IN NO EVENT SHALL J. Irving Vasquez-Gomez BE LIABLE FOR ANY
 20 | % DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 21 | % (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 22 | % LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 23 | % ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 24 | % (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 25 | % SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 26 | 
 27 | 
 28 | % Best path for antipodal pairs
 29 | % A: antipodal pairs
 30 | % V: vertices of the polygon
 31 | % 
 32 | 
 33 | function [Path, inclination] = bestPathForAntipodalPair(V, A, dx) 
 34 |     [n,~] = size(V);
 35 | 
 36 |     i = A(1,1);
 37 |     j = A(1,2);
 38 | 
 39 |     %if angleAP(V, i,j) < angleAP(V, j, i)
 40 |     if (angleAP(V,i,j) - pi) < 0
 41 |         b1 = j;
 42 |         a1 = i;
 43 |     else
 44 |         b1 = i;
 45 |         a1 = j;
 46 |     end
 47 | 
 48 |     phi = angleAP(V, b1, a1)-pi;
 49 |     gamma_b = angleAP(V, decrement(b1,n), b1);
 50 |     gamma_a = angleAP(V, decrement(a1,n), a1) - phi;
 51 |     if gamma_b < gamma_a 
 52 |         b2 = decrement(b1,n);
 53 |         a2 = a1;
 54 |     else
 55 |         b2 = decrement(a1,n);
 56 |         a2 = b1;
 57 |     end
 58 |     
 59 |     d1 = distPoint2Line(V(b1,:),V(increment(b1,n),:),V(a1,:));
 60 |     d2 = distPoint2Line(V(b2,:),V(increment(b2,n),:),V(a2,:));
 61 |     
 62 |     if d1 < d2
 63 |         b_vertex = b1;
 64 |         c_vertex = increment(b1,n);
 65 |         inclination = slopeAngle(V(b_vertex,:),V(c_vertex,:));       
 66 |              
 67 |         Pstart = V;
 68 |         Pend = circshift(V, -1);
 69 | %         figure; axis equal;
 70 | %         hold on;
 71 | %         line([Pstart(:,1)';Pend(:,1)'],[Pstart(:,2)';Pend(:,2)'],'Color','k');
 72 | %         scatter(Pstart(b1,1), Pstart(b1,2), 20, 'filled');
 73 | %         scatter(Pstart(c_vertex,1), Pstart(c_vertex,2), 20, 'filled');
 74 | %         title('Polygon in best path');
 75 | %         hold off;
 76 | 
 77 |         theta = pi/2 - inclination; % R rotation is with respect of the east; alpha rotation is with respect of the north
 78 |         
 79 |         PsR = rotatePolygon(Pstart, theta);
 80 |         PsR = PsR';
 81 |         PeR = rotatePolygon(Pend, theta);
 82 |         PeR = PeR';
 83 | 
 84 | %         figure; axis equal;
 85 | %         hold on;
 86 | %         line([PsR(:,1)';PeR(:,1)'],[PsR(:,2)';PeR(:,2)'],'Color','k');
 87 | %         scatter(PsR(b1,1), PsR(b1,2), 20, 'filled');
 88 | %         scatter(PsR(c_vertex,1), PsR(c_vertex,2), 20, 'filled');
 89 | %         title('Polygon rotated in best path');
 90 | 
 91 |         [PathRotated] = getPathMR([PsR PeR], dx, 1);
 92 | %         plot(PathRotated(:,1), PathRotated(:,2));
 93 | %         hold off;
 94 | %         pause;
 95 |         Path = rotatePolygon(PathRotated, -theta);
 96 |         Path = Path';
 97 |     else
 98 |         b_vertex = increment(b2,n);
 99 |         c_vertex = b2;
100 |         inclination = slopeAngle(V(b_vertex,:),V(c_vertex,:));
101 | %        inclination * 180 / pi
102 | %        R(k) = inclination;
103 | 
104 |         Pstart = V;
105 |         Pend = circshift(V, -1);
106 | %         figure; axis equal;
107 | %         hold on;
108 | %         line([Pstart(:,1)';Pend(:,1)'],[Pstart(:,2)';Pend(:,2)'],'Color','k');
109 | %         scatter(Pstart(b_vertex,1), Pstart(b_vertex,2), 20, 'filled');
110 | %         scatter(Pstart(c_vertex,1), Pstart(c_vertex,2), 20, 'filled');
111 | %         title('Polygon in best path inv');
112 | %         hold off;
113 | 
114 |         theta = -pi/2 - inclination; % R rotation is with respect of the east; alpha rotation is with respect of the north
115 |         PsR = rotatePolygon(Pstart, theta);
116 |         PsR = PsR';
117 |         PeR = rotatePolygon(Pend, theta);
118 |         PeR = PeR';
119 | 
120 | %         figure; axis equal;
121 | %         hold on;
122 | %         line([PsR(:,1)';PeR(:,1)'],[PsR(:,2)';PeR(:,2)'],'Color','k');
123 | %         scatter(PsR(b_vertex,1), PsR(b_vertex,2), 20, 'filled');
124 | %         scatter(PsR(c_vertex,1), PsR(c_vertex,2), 20, 'filled');
125 | %         title('Polygon rotated in best path inv');
126 | 
127 | 
128 |         [PathRotated] = getPathMR([PsR PeR], dx, -1);
129 | %         plot(PathRotated(:,1), PathRotated(:,2));
130 | %         hold off;
131 | %         pause;
132 |         Path = rotatePolygon(PathRotated, -theta);
133 |         Path = Path';
134 |     end
135 | end
136 | 
137 | function i_next = increment(i,n)
138 |     i_next = mod(i,n)+1;
139 | end
140 | 
141 | function i_prev = decrement(i,n)
142 |     i_prev = i-1;
143 |     if i_prev <1
144 |         i_prev = n;
145 |     end
146 | end
147 | 
148 | function alpha = slopeAngle(p1, p2)
149 |     alpha = atan2(p2(2)-p1(2),p2(1)-p1(1));
150 | end
151 | 


--------------------------------------------------------------------------------
/data/p_e_rcpp.txt:
--------------------------------------------------------------------------------
1 | 7.100000000000000000e+01
2 | 3.450000000000000000e+02
3 | 


--------------------------------------------------------------------------------
/data/p_x_rcpp.txt:
--------------------------------------------------------------------------------
1 | 7.100000000000000000e+01
2 | 3.450000000000000000e+02
3 | 


--------------------------------------------------------------------------------
/data/target_rcpp_room.txt:
--------------------------------------------------------------------------------
1 | 1.000000000000000000e+01,3.450000000000000000e+02
2 | 1.790000000000000000e+02,3.450000000000000000e+02
3 | 1.790000000000000000e+02,5.960000000000000000e+02
4 | 1.000000000000000000e+01,5.960000000000000000e+02
5 | 1.000000000000000000e+01,3.450000000000000000e+02
6 | 


--------------------------------------------------------------------------------
/distPoint2Line.m:
--------------------------------------------------------------------------------
 1 | % CONACYT
 2 | % Copyright (c) 2017, Juan Irving Vasquez-Gomez,
 3 | % All rights reserved.
 4 | % 
 5 | % Redistribution and use in source and binary forms, with or without
 6 | % modification, are permitted provided that the following conditions are met:
 7 | %     * Redistributions of source code must retain the above copyright
 8 | %       notice, this list of conditions and the following disclaimer.
 9 | %     * Redistributions in binary form must reproduce the above copyright
10 | %       notice, this list of conditions and the following disclaimer in the
11 | %       documentation and/or other materials provided with the distribution.
12 | %     * Neither the name of the <organization> nor the
13 | %       names of its contributors may be used to endorse or promote products
14 | %       derived from this software without specific prior written permission.
15 | % 
16 | % THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 | % ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 | % WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 | % DISCLAIMED. IN NO EVENT SHALL J. Irving Vasquez-Gomez BE LIABLE FOR ANY
20 | % DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 | % (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 | % LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 | % ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 | % (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 | % SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 | 
27 | 
28 | % Line pass trough pi and pa
29 | % pa means p adjacent to pi
30 | function dist = distPoint2Line(Pi,Pa,Pj)
31 |     xi = Pi(1);
32 |     yi = Pi(2);
33 |     
34 |     xa = Pa(1);
35 |     ya = Pa(2);
36 |     
37 |     xj = Pj(1);
38 |     yj = Pj(2);
39 |     
40 |     dist = abs( (ya-yi)*xj -(xa-xi)*yj + xa*yi-xi*ya  )/ sqrt((ya-yi)*(ya-yi) + (xa-xi)*(xa-xi));
41 | end


--------------------------------------------------------------------------------
/getConvexPolygon.m:
--------------------------------------------------------------------------------
 1 | % CONACYT
 2 | % Copyright (c) 2017, Juan Irving Vasquez-Gomez,
 3 | % All rights reserved.
 4 | % 
 5 | % Redistribution and use in source and binary forms, with or without
 6 | % modification, are permitted provided that the following conditions are met:
 7 | %     * Redistributions of source code must retain the above copyright
 8 | %       notice, this list of conditions and the following disclaimer.
 9 | %     * Redistributions in binary form must reproduce the above copyright
10 | %       notice, this list of conditions and the following disclaimer in the
11 | %       documentation and/or other materials provided with the distribution.
12 | %     * Neither the name of the <organization> nor the
13 | %       names of its contributors may be used to endorse or promote products
14 | %       derived from this software without specific prior written permission.
15 | % 
16 | % THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 | % ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 | % WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 | % DISCLAIMED. IN NO EVENT SHALL J. Irving Vasquez-Gomez BE LIABLE FOR ANY
20 | % DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 | % (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 | % LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 | % ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 | % (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 | % SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 | 
27 | function [Polygon_vertex, shifted_polygon_vertex] = getConvexPolygon(numVert, radius, radVar, angVar)
28 | 
29 | isConvex = false;
30 | 
31 | while not(isConvex)
32 |     [Polygon_vertex, shifted_polygon_vertex]= getPolygon(numVert, radius, radVar, angVar);
33 |     isConvex = isAConvexPolygon(Polygon_vertex);
34 | end
35 | 
36 | end


--------------------------------------------------------------------------------
/getPathMR.m:
--------------------------------------------------------------------------------
  1 | % CONACYT
  2 | % Copyright (c) 2017, J. Irving Vasquez-Gomez,
  3 | % All rights reserved.
  4 | % 
  5 | % Redistribution and use in source and binary forms, with or without
  6 | % modification, are permitted provided that the following conditions are met:
  7 | %     * Redistributions of source code must retain the above copyright
  8 | %       notice, this list of conditions and the following disclaimer.
  9 | %     * Redistributions in binary form must reproduce the above copyright
 10 | %       notice, this list of conditions and the following disclaimer in the
 11 | %       documentation and/or other materials provided with the distribution.
 12 | %     * Neither the name of the <organization> nor the
 13 | %       names of its contributors may be used to endorse or promote products
 14 | %       derived from this software without specific prior written permission.
 15 | % 
 16 | % THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 17 | % ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 18 | % WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 19 | % DISCLAIMED. IN NO EVENT SHALL J. Irving Vasquez-Gomez BE LIABLE FOR ANY
 20 | % DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 21 | % (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 22 | % LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 23 | % ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 24 | % (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 25 | % SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 26 | 
 27 | function [Path] = getPathMR(M, dx, dir)
 28 | % Returns a path for a given CONVEX polygon 
 29 | % This is for a multirotor UAV, namely it generates sharp corners
 30 | % Parameters: 
 31 | % M: plygon. M should be formed by:
 32 | % [px0 py0 px1 py1]
 33 | % [px1 py1 px2 py2]
 34 | % . 
 35 | % .
 36 | % [pxn pyn px0 py0]
 37 | % 
 38 | % dx: distance between fligh lines
 39 | % dir: direction of the initial flight line. 1: forth (abajo arriba), -1:
 40 | % back (arriba abajo). Default dir = 1% 
 41 | 
 42 | % Returns only the waypoints
 43 | % For computing distances use getPathMR2
 44 | 
 45 | %% Generate lines
 46 | gap_y = 1;
 47 | 
 48 | % polygon limits
 49 | l_limit = min(M(:,1));
 50 | r_limit = max(M(:,1));
 51 | down_limit = min(M(:,2));
 52 | up_limit = max(M(:,2));
 53 | 
 54 | % extend the lines beyond the polygon
 55 | y1 = down_limit - gap_y;
 56 | y2 = up_limit + gap_y;
 57 | 
 58 | x1 = l_limit + dx/2;
 59 | x2 = x1;
 60 | 
 61 | %dir = 1;% 1: forth (abajo arriba), -1: back (arriba abajo)
 62 | 
 63 | %% Show the intersection points.
 64 | %     figure('Position',[10 100 500 500],'Renderer','zbuffer'); 
 65 | %     axes_properties.box             = 'on';
 66 | %     axes_properties.XLim            = [-2 2];
 67 | %     axes_properties.YLim            = [-2 2];
 68 | %     axes_properties.DataAspectRatio = [1 1 1];
 69 | %     axes_properties.NextPlot        = 'add';
 70 | %     axes(axes_properties,'parent',gcf);
 71 | %     hold on
 72 |   
 73 | %%
 74 | 
 75 | i = 1; %numero de waypoints agregados
 76 | lines = 0;
 77 | %nLinesF = 0;
 78 | %nLinesB = 0;
 79 | enterWP = [0 0];
 80 | exitWP = [0 0];
 81 | lastWP = [0 0];
 82 | 
 83 | while(x1 < (r_limit + dx/2)) %% TEST WARNING!!! previous: while(x1 <= r_limit)
 84 |     % linea de intersección
 85 |     LineXY = [x1 y1 x2 y2];
 86 |     lines = lines +1;
 87 |     
 88 |     %intersecciones
 89 |     out = lineSegmentIntersect(M,LineXY);
 90 |     intersections = [out.intMatrixX(:) out.intMatrixY(:)];
 91 |     intersections( ~any(intersections,2), : ) = [];  
 92 |     n = size(intersections,1);  
 93 |     
 94 |     % Si solo hay una intersección agregamos el punto directamente
 95 |     if (n == 1)
 96 |         p1 = intersections(1,:);
 97 |         Path(i,:) = p1;
 98 |         lastWP = p1;
 99 |         i = i + 1;
100 |     end
101 |     
102 |     % si hay dos intersecciones entonces tomamos las intersecciones como wp
103 |     if (n > 1)
104 |         
105 |         if(n>2)
106 |            [B, kkk] = sort(intersections(:,2)); % TODO: Revisar que esto este bien
107 |            ordered = [intersections(kkk) B];
108 |            p1 = ordered(1,:);
109 |            p2 = ordered(n,:);
110 |         else
111 |             p1 = intersections(1,:);
112 |             p2 = intersections(2,:);
113 |         end
114 |             
115 |         % las ordenamos de acuerdo a la dirección
116 |         if (dir == 1) % direcciń hacia arriba
117 |             
118 |             % ordenar los puntos para que corresponda con la dirección
119 |             % hacia arriba
120 |             if (p2(1,2)<p1(1,2))
121 |                 enterWP = p2; % punto de entrada a la linea
122 |                 exitWP = p1; % punto de salida de la linea
123 |             else
124 |                 enterWP = p1;
125 |                 exitWP = p2;
126 |             end
127 |             
128 |             % si hay lineas previas entonces ajustamos los nuevos waypoints con los waypoints the la linea previa
129 |             if(lines > 1)
130 |                 dif = enterWP(1,2) - lastWP(1,2);
131 |                 if(dif < 0) 
132 |                     % Si la diferencia es negativa entonces el último punto
133 |                     % está arriba del punto de entrada.
134 |                     % Entones reemplazar el último punto.
135 |                     intermediateWP = [lastWP(1,1) enterWP(1,2)];
136 |                     Path(i-1,:) = intermediateWP;
137 |                 else
138 |                     % De lo contrario, mover el punto de entradada
139 |                     intermediateWP = [enterWP(1,1) lastWP(1,2)];
140 |                     enterWP = intermediateWP;
141 |                 end
142 |             end
143 |             
144 |             % Agregar puntos de la nueva linea
145 |             Path(i,:) = enterWP;
146 |             i = i+1;
147 |             Path(i,:) = exitWP;
148 |             i = i+1;
149 |             
150 |         else % dirección hacia abajo
151 |            %ordenar los waypoints
152 |            if (p2(1,2)<p1(1,2)) 
153 |                enterWP = p1;
154 |                exitWP = p2;
155 |            else
156 |                enterWP = p2;
157 |                exitWP = p1;
158 |            end
159 |            
160 |            if(lines > 1)
161 |                 % Alinear el ultimo punto y el punto de entrada
162 |                 dif = enterWP(1,2) - lastWP(1,2);
163 |                 if(dif > 0) 
164 |                     % Modificar el ultimo punto
165 |                     intermediateWP = [lastWP(1,1) enterWP(1,2)];
166 |                     Path(i-1,:) = intermediateWP;
167 |                 else
168 |                     % Modificar el punto de entrada
169 |                     intermediateWP = [enterWP(1,1) lastWP(1,2)];
170 |                     enterWP = intermediateWP;      
171 |                 end
172 |             end
173 |             Path(i,:) = enterWP;
174 |             i = i+1;
175 |             Path(i,:) = exitWP;
176 |             i = i+1;
177 | 
178 |         end
179 |         
180 |         dir = dir * -1;
181 |         lastWP = exitWP;
182 |     end
183 |     
184 |     % Si no hay intersecciones ya salio del polígono pero una parte del
185 |     % polígono no ha sido vista por la mitad del FOV
186 |     if (n==0)
187 |         %regresamos la linea una distancia igual a la mitad del FOV
188 |         % linea de intersección
189 |         LineFOV = [x1-dx/2 y1 x2-dx/2 y2];
190 |         
191 |         % Verificar la intersección del FOV
192 |         %intersecciones
193 |         out = lineSegmentIntersect(M, LineFOV);
194 |         intersections = [out.intMatrixX(:) out.intMatrixY(:)];
195 |         intersections( ~any(intersections,2), : ) = [];  
196 |         n = size(intersections,1);
197 |         
198 |         %si hubo mas de una intersección
199 |         if(n > 1)
200 |             
201 |             if(n>2)
202 |                [B, kkk] = sort(intersections(:,2)); % TODO: Revisar que esto este bien
203 |                ordered = [intersections(kkk) B];
204 |                p1 = ordered(1,:);
205 |                p2 = ordered(n,:);
206 |             else
207 |                 p1 = intersections(1,:);
208 |                 p2 = intersections(2,:);
209 |             end
210 | 
211 |             % ajustar las intersecciones para que la linea que se agrege
212 |             % quede fuera del poligono
213 |             p1(1,1) = p1(1,1) + dx/2;
214 |             p2(1,1) = p2(1,1) + dx/2;
215 | 
216 |             % las ordenamos de acuerdo a la dirección
217 |             if (dir == 1) % direcciń hacia arriba
218 | 
219 |                 % ordenar los puntos para que corresponda con la dirección
220 |                 % hacia arriba
221 |                 if (p2(1,2)<p1(1,2))
222 |                     enterWP = p2; % punto de entrada a la linea
223 |                     exitWP = p1; % punto de salida de la linea
224 |                 else
225 |                     enterWP = p1;
226 |                     exitWP = p2;
227 |                 end
228 | 
229 |                 % si hay lineas previas entonces ajustamos los nuevos waypoints con los waypoints the la linea previa
230 |                 if(lines > 1)
231 |                     dif = enterWP(1,2) - lastWP(1,2);
232 |                     if(dif < 0) 
233 |                         % Si la diferencia es negativa entonces el último punto
234 |                         % está arriba del punto de entrada.
235 |                         % Entones reemplazar el último punto.
236 |                         intermediateWP = [lastWP(1,1) enterWP(1,2)];
237 |                         Path(i-1,:) = intermediateWP;
238 |                     else
239 |                         % De lo contrario, mover el punto de entradada
240 |                         intermediateWP = [enterWP(1,1) lastWP(1,2)];
241 |                         enterWP = intermediateWP;
242 |                     end
243 |                 end
244 | 
245 |                 % Agregar puntos de la nueva linea
246 |                 Path(i,:) = enterWP;
247 |                 i = i+1;
248 |                 Path(i,:) = exitWP;
249 |                 i = i+1;
250 | 
251 |             else % dirección hacia abajo
252 | 
253 |                % ordenar los waypoints
254 |                if (p2(1,2)<p1(1,2)) 
255 |                    enterWP = p1;
256 |                    exitWP = p2;
257 |                else
258 |                    enterWP = p2;
259 |                    exitWP = p1;
260 |                end
261 | 
262 |                if(lines > 1)
263 |                     % Alinear el ultimo punto y el punto de entrada
264 |                     dif = enterWP(1,2) - lastWP(1,2);
265 |                     if(dif > 0) 
266 |                         % Modificar el ultimo punto
267 |                         intermediateWP = [lastWP(1,1) enterWP(1,2)];
268 |                         Path(i-1,:) = intermediateWP;
269 |                     else
270 |                         % Modificar el punto de entrada
271 |                         intermediateWP = [enterWP(1,1) lastWP(1,2)];
272 |                         enterWP = intermediateWP;      
273 |                     end
274 |                 end
275 |                 Path(i,:) = enterWP;
276 |                 i = i+1;
277 |                 Path(i,:) = exitWP;
278 |                 i = i+1;
279 | 
280 |             end
281 | 
282 |             dir = dir * -1;
283 |             lastWP = exitWP;
284 |         end
285 |     end
286 |     
287 |     %nLines = [nLinesB nLinesF];
288 |     %pause
289 |     
290 |     x1 = x1 + dx;
291 |     x2 = x1;
292 | end
293 | 
294 | % title('Intersection Points');
295 | % hold off;
296 | 
297 | end


--------------------------------------------------------------------------------
/getPathMR2.m:
--------------------------------------------------------------------------------
  1 | % CONACYT
  2 | % Copyright (c) 2017, J. Irving Vasquez-Gomez,
  3 | % All rights reserved.
  4 | % 
  5 | % Redistribution and use in source and binary forms, with or without
  6 | % modification, are permitted provided that the following conditions are met:
  7 | %     * Redistributions of source code must retain the above copyright
  8 | %       notice, this list of conditions and the following disclaimer.
  9 | %     * Redistributions in binary form must reproduce the above copyright
 10 | %       notice, this list of conditions and the following disclaimer in the
 11 | %       documentation and/or other materials provided with the distribution.
 12 | %     * Neither the name of the CONACYT nor the
 13 | %       names of its contributors may be used to endorse or promote products
 14 | %       derived from this software without specific prior written permission.
 15 | % 
 16 | % THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 17 | % ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 18 | % WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 19 | % DISCLAIMED. IN NO EVENT SHALL J. Irving Vasquez-Gomez BE LIABLE FOR ANY
 20 | % DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 21 | % (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 22 | % LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 23 | % ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 24 | % (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 25 | % SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 26 | 
 27 | 
 28 | 
 29 | % default dir = 1% 
 30 | % 1: forth (abajo arriba), -1: back (arriba abajo)
 31 | % 
 32 | function [Path, Dist] = getPathMR2(M, dx, dir)
 33 | % Returns a path for a given polygon 
 34 | % This is for a multirotor UAV, namely it generates sharp corners
 35 | % Receives a poygon in M, M should be formed by:
 36 | % [px0 py0 px1 py1]
 37 | % [px1 py1 px2 py2]
 38 | % . 
 39 | % .
 40 | % [pxn pyn px0 py0]
 41 | 
 42 | % Returns the waypoints and the distances
 43 | % Dist = [lb lf ll lr];
 44 | % Solo para poligonos convexos
 45 | % BUG que pasa si hay mas de una intersección con la linea vertical % 
 46 | 
 47 | %% Generate lines
 48 | 
 49 | gap_y = 1;
 50 | 
 51 | % polygon limits
 52 | l_limit = min(M(:,1));
 53 | r_limit = max(M(:,1));
 54 | down_limit = min(M(:,2));
 55 | up_limit = max(M(:,2));
 56 | 
 57 | % extend the lines beyond the polygon
 58 | y1 = down_limit - gap_y;
 59 | y2 = up_limit + gap_y;
 60 | 
 61 | x1 = l_limit + dx/2;
 62 | x2 = x1;
 63 | 
 64 | df = 0;
 65 | db = 0;
 66 | dr = 0;
 67 | dl = 0;
 68 | %dir = 1;% 1: forth (abajo arriba), -1: back (arriba abajo)
 69 | 
 70 | %% Show the intersection points.
 71 | %     figure('Position',[10 100 500 500],'Renderer','zbuffer'); 
 72 | %     axes_properties.box             = 'on';
 73 | %     axes_properties.XLim            = [-2 2];
 74 | %     axes_properties.YLim            = [-2 2];
 75 | %     axes_properties.DataAspectRatio = [1 1 1];
 76 | %     axes_properties.NextPlot        = 'add';
 77 | %     axes(axes_properties,'parent',gcf);
 78 | %     hold on
 79 |   
 80 | %%
 81 | 
 82 | i = 1; %numero de waypoints agregados
 83 | lines = 0;
 84 | nLinesF = 0;
 85 | nLinesB = 0;
 86 | enterWP = [0 0];
 87 | exitWP = [0 0];
 88 | lastWP = [0 0];
 89 | 
 90 | while(x1 <= (r_limit + dx/2)) %% TEST WARNING!!! previous: while(x1 <= r_limit)
 91 |     % linea de intersección
 92 |     LineXY = [x1 y1 x2 y2];
 93 |     lines = lines +1;
 94 |     
 95 |     %intersecciones
 96 |     out = lineSegmentIntersect(M,LineXY);
 97 |     intersections = [out.intMatrixX(:) out.intMatrixY(:)];
 98 |     intersections( ~any(intersections,2), : ) = [];  
 99 |     n = size(intersections,1);  
100 |     
101 |     % Si solo hay una intersección agregamos el punto directamente
102 |     if (n == 1)
103 |         p1 = intersections(1,:);
104 |         Path(i,:) = p1;
105 |         enterWP = p1;
106 |         i = i+1;
107 |     end
108 |     
109 |     % si hay dos intersecciones entonces tomamos las intersecciones como wp
110 |     if (n > 1)
111 |         
112 |         if(n>2)
113 |            [B, kkk] = sort(intersections(:,2)); % TODO: Revisar que esto este bien
114 |            ordered = [intersections(kkk) B];
115 |            p1 = ordered(1,:);
116 |            p2 = ordered(n,:);
117 |         else
118 |             p1 = intersections(1,:);
119 |             p2 = intersections(2,:);
120 |         end
121 |             
122 |             
123 |         % las ordenamos de acuerdo a la dirección
124 |         if (dir == 1) % direcciń hacia arriba
125 |             
126 |             % ordenar los puntos para que corresponda con la dirección
127 |             % hacia arriba
128 |             if (p2(1,2)<p1(1,2))
129 |                 enterWP = p2; % punto de entrada a la linea
130 |                 exitWP = p1; % punto de salida de la linea
131 |             else
132 |                 enterWP = p1;
133 |                 exitWP = p2;
134 |             end
135 |             
136 |             % si hay lineas previas entonces conectamos la linea previa con
137 |             % la nueva a través de curvas de dubins
138 |             if(lines > 1)
139 |                 % agregar un punto para compensar la inclinación de la
140 |                 % arista del poligono
141 |                 dif = enterWP(1,2) - lastWP(1,2);
142 |                 if(dif < 0) % agregar un punto a la izquierda
143 |                     intermediateWP = [lastWP(1,1) enterWP(1,2)];
144 |                     Path(i,:) = intermediateWP;
145 |                     i = i+1;
146 |                     
147 |                     % TODO: integrar la energia gastada entre esos dos
148 |                     % puntos
149 |                     %[dubinsWP, dist] = getDubinsWaypoints(intermediateWP, enterWP, curve_radius, dx, -1);
150 |                     Path = [Path ;intermediateWP; enterWP];
151 |                     %i = i + size(dubinsWP,1);
152 |                     i = i + 2;
153 |                     dist = norm(intermediateWP-enterWP);
154 |                     dl = dl + dist;
155 |                 else % agregar un punto a la derecha
156 |                     intermediateWP = [enterWP(1,1) lastWP(1,2)];
157 |                     
158 |                     %[dubinsWP, dist] = getDubinsWaypoints(lastWP, intermediateWP, curve_radius, dx, -1);
159 |                     %Path = [Path ;dubinsWP];
160 |                     Path = [Path; lastWP; intermediateWP];
161 |                     %i = i + size(dubinsWP,1);
162 |                     i = i + 2;
163 |                     
164 |                     %Path(i,:) = intermediateWP;
165 |                     %i = i+1;
166 |                     
167 |                     dist = norm(lastWP-intermediateWP);
168 |                     dl = dl + dist;
169 |                 end
170 |             end
171 |             
172 |             Path(i,:) = enterWP;
173 |             i = i+1;
174 |             Path(i,:) = exitWP;
175 |             i = i+1;
176 |             
177 |             dist = norm(exitWP-enterWP);
178 |             df = df + dist;
179 |             nLinesF = nLinesF +1;
180 |         else % dirección hacia abajo
181 |             
182 |            %ordenar los waypoints
183 |            if (p2(1,2)<p1(1,2)) 
184 |                enterWP = p1;
185 |                exitWP = p2;
186 |            else
187 |                enterWP = p2;
188 |                exitWP = p1;
189 |            end
190 |            
191 |            if(lines > 1)
192 |                 % agregar un punto para compensar
193 |                 dif = enterWP(1,2) - lastWP(1,2);
194 |                 if(dif > 0) % agregar un punto a la izquierda
195 |                     intermediateWP = [lastWP(1,1) enterWP(1,2)];
196 |                     
197 |                     %Path(i,:) = intermediateWP;
198 |                     %i = i+1;
199 | 
200 |                     %[dubinsWP, dist] = getDubinsWaypoints(intermediateWP, enterWP, curve_radius, dx, 1);
201 |                     %Path = [Path ;dubinsWP];
202 |                     Path = [Path; intermediateWP; enterWP];
203 |                     %i = i + size(dubinsWP,1);
204 |                     i = i+2;
205 |                     
206 |                     dist = norm(intermediateWP-enterWP);
207 |                     dr = dr + dist;
208 |                 else % agregar un punto a la derecha
209 |                     intermediateWP = [enterWP(1,1) lastWP(1,2)];
210 |                                                             
211 |                     
212 |                     %[dubinsWP, dist] = getDubinsWaypoints(lastWP, intermediateWP, curve_radius, dx, 1);
213 |                     %Path = [Path ;dubinsWP];
214 |                     Path = [Path; lastWP; intermediateWP];
215 |                     %i = i + size(dubinsWP,1);
216 |                     i = i+2;
217 |                     
218 |                     %Path(i,:) = intermediateWP;
219 |                     %i = i+1;
220 |                     dist = norm(lastWP-intermediateWP);
221 |                     dr = dr + dist;
222 |                 end
223 |             end
224 |            
225 |             Path(i,:) = enterWP;
226 |             i = i+1;
227 |             Path(i,:) = exitWP;
228 |             i = i+1;
229 |             
230 |             dist = norm(exitWP-enterWP);
231 |             db = db + dist;
232 |             nLinesB = nLinesB +1 ;
233 |         end
234 |         
235 |         dir = dir * -1;
236 |         lastWP = exitWP;
237 |     end
238 |     
239 |     % Si no hay intersecciones ya salio del polígono pero una parte del
240 |     % polígono no ha sido vista por la mitad del FOV
241 |     if (n==0)
242 |         %regresamos la linea una distancia igual a la mitad del FOV
243 |         % linea de intersección
244 |         LineFOV = [x1-dx/2 y1 x2-dx/2 y2];
245 |         
246 |         % Verificar la intersección del FOV
247 |         %intersecciones
248 |         out = lineSegmentIntersect(M, LineFOV);
249 |         intersections = [out.intMatrixX(:) out.intMatrixY(:)];
250 |         intersections( ~any(intersections,2), : ) = [];  
251 |         n = size(intersections,1);
252 |         
253 |         %si hubo mas de una intersección
254 |         if(n > 1)
255 |             
256 |             if(n>2)
257 |                [B, kkk] = sort(intersections(:,2)); % TODO: Revisar que esto este bien
258 |                ordered = [intersections(kkk) B];
259 |                p1 = ordered(1,:);
260 |                p2 = ordered(n,:);
261 |             else
262 |                 p1 = intersections(1,:);
263 |                 p2 = intersections(2,:);
264 |             end
265 | 
266 |             % ajustar las intersecciones para que la linea que se agrege
267 |             % quede fuera del poligono
268 |             p1(1,1) = p1(1,1) + dx/2;
269 |             p2(1,1) = p2(1,1) + dx/2;
270 | 
271 |             % las ordenamos de acuerdo a la dirección
272 |             if (dir == 1) % direcciń hacia arriba
273 | 
274 |                 % ordenar los puntos para que corresponda con la dirección
275 |                 % hacia arriba
276 |                 if (p2(1,2)<p1(1,2))
277 |                     enterWP = p2; % punto de entrada a la linea
278 |                     exitWP = p1; % punto de salida de la linea
279 |                 else
280 |                     enterWP = p1;
281 |                     exitWP = p2;
282 |                 end
283 | 
284 |                 % si hay lineas previas entonces conectamos la linea previa con
285 |                 % la nueva a través de curvas de dubins
286 |                 if(lines > 1)
287 |                     % agregar un punto para compensar la inclinación de la
288 |                     % arista del poligono
289 |                     dif = enterWP(1,2) - lastWP(1,2);
290 |                     if(dif < 0) % agregar un punto a la izquierda
291 |                         intermediateWP = [lastWP(1,1) enterWP(1,2)];
292 |                         Path(i,:) = intermediateWP;
293 |                         i = i+1;
294 | 
295 |                         % TODO: integrar la energia gastada entre esos dos
296 |                         % puntos
297 |                         %[dubinsWP, dist] = getDubinsWaypoints(intermediateWP, enterWP, curve_radius, dx, -1);
298 |                         Path = [Path ;intermediateWP; enterWP];
299 |                         %i = i + size(dubinsWP,1);
300 |                         i = i + 2;
301 |                         dist = norm(intermediateWP-enterWP);
302 |                         dl = dl + dist;
303 |                     else % agregar un punto a la derecha
304 |                         intermediateWP = [enterWP(1,1) lastWP(1,2)];
305 | 
306 |                         %[dubinsWP, dist] = getDubinsWaypoints(lastWP, intermediateWP, curve_radius, dx, -1);
307 |                         %Path = [Path ;dubinsWP];
308 |                         Path = [Path; lastWP; intermediateWP];
309 |                         %i = i + size(dubinsWP,1);
310 |                         i = i + 2;
311 | 
312 |                         %Path(i,:) = intermediateWP;
313 |                         %i = i+1;
314 | 
315 |                         dist = norm(lastWP-intermediateWP);
316 |                         dl = dl + dist;
317 |                     end
318 |                 end
319 | 
320 |                 Path(i,:) = enterWP;
321 |                 i = i+1;
322 |                 Path(i,:) = exitWP;
323 |                 i = i+1;
324 | 
325 |                 dist = norm(exitWP-enterWP);
326 |                 df = df + dist;
327 |                 nLinesF = nLinesF +1;
328 |             else % dirección hacia abajo
329 | 
330 |                %ordenar los waypoints
331 |                if (p2(1,2)<p1(1,2)) 
332 |                    enterWP = p1;
333 |                    exitWP = p2;
334 |                else
335 |                    enterWP = p2;
336 |                    exitWP = p1;
337 |                end
338 | 
339 |                if(lines > 1)
340 |                     % agregar un punto para compensar
341 |                     dif = enterWP(1,2) - lastWP(1,2);
342 |                     if(dif > 0) % agregar un punto a la izquierda
343 |                         intermediateWP = [lastWP(1,1) enterWP(1,2)];
344 | 
345 |                         %Path(i,:) = intermediateWP;
346 |                         %i = i+1;
347 | 
348 |                         %[dubinsWP, dist] = getDubinsWaypoints(intermediateWP, enterWP, curve_radius, dx, 1);
349 |                         %Path = [Path ;dubinsWP];
350 |                         Path = [Path; intermediateWP; enterWP];
351 |                         %i = i + size(dubinsWP,1);
352 |                         i = i+2;
353 | 
354 |                         dist = norm(intermediateWP-enterWP);
355 |                         dr = dr + dist;
356 |                     else % agregar un punto a la derecha
357 |                         intermediateWP = [enterWP(1,1) lastWP(1,2)];
358 | 
359 | 
360 |                         %[dubinsWP, dist] = getDubinsWaypoints(lastWP, intermediateWP, curve_radius, dx, 1);
361 |                         %Path = [Path ;dubinsWP];
362 |                         Path = [Path; lastWP; intermediateWP];
363 |                         %i = i + size(dubinsWP,1);
364 |                         i = i+2;
365 | 
366 |                         %Path(i,:) = intermediateWP;
367 |                         %i = i+1;
368 |                         dist = norm(lastWP-intermediateWP);
369 |                         dr = dr + dist;
370 |                     end
371 |                 end
372 | 
373 |                 Path(i,:) = enterWP;
374 |                 i = i+1;
375 |                 Path(i,:) = exitWP;
376 |                 i = i+1;
377 | 
378 |                 dist = norm(exitWP-enterWP);
379 |                 db = db + dist;
380 |                 nLinesB = nLinesB +1 ;
381 |             end
382 | 
383 |             dir = dir * -1;
384 |             lastWP = exitWP;
385 |         end
386 |     end
387 |     
388 |     
389 |     nLines = [nLinesB nLinesF];
390 |     %pause
391 |     
392 |     x1 = x1 + dx;
393 |     x2 = x1;
394 | end
395 | 
396 | % title('Intersection Points');
397 | % hold off;
398 | 
399 | Dist = [db df dl dr];
400 | 
401 | 
402 | end


--------------------------------------------------------------------------------
/getPolygon.m:
--------------------------------------------------------------------------------
 1 | % CONACYT
 2 | % Copyright (c) 2017, J. Irving Vasquez-Gomez,
 3 | % All rights reserved.
 4 | % 
 5 | % Redistribution and use in source and binary forms, with or without
 6 | % modification, are permitted provided that the following conditions are met:
 7 | %     * Redistributions of source code must retain the above copyright
 8 | %       notice, this list of conditions and the following disclaimer.
 9 | %     * Redistributions in binary form must reproduce the above copyright
10 | %       notice, this list of conditions and the following disclaimer in the
11 | %       documentation and/or other materials provided with the distribution.
12 | %     * Neither the name of the <organization> nor the
13 | %       names of its contributors may be used to endorse or promote products
14 | %       derived from this software without specific prior written permission.
15 | % 
16 | % THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 | % ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 | % WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 | % DISCLAIMED. IN NO EVENT SHALL J. Irving Vasquez-Gomez BE LIABLE FOR ANY
20 | % DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 | % (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 | % LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 | % ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 | % (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 | % SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 | 
27 | 
28 | 
29 | function [Polygon_vertex, shifted_polygon_vertex] = getPolygon(numVert, radius, radVar, angVar)
30 | % Generates a convex polygon
31 | 
32 | % Specify polygon variables
33 | %numVert = 8;
34 | %radius = 1;
35 | %radVar = 1; % variance in the spikiness of vertices
36 | %angVar = 1; % variance in spacing of vertices around the unit circle
37 | 
38 | % preallocate x and y 
39 | x = zeros(numVert,1);
40 | y = zeros(numVert,1);
41 | 
42 | % angle of the unit circle in radians
43 | circleAng = 2*pi;
44 | % the average angular separation between points in a unit circle
45 | angleSeparation = circleAng/double(numVert);
46 | % create the matrix of angles for equal separation of points
47 | angleMatrix = 0: angleSeparation: circleAng;
48 | % drop the final angle since 2Pi = 0
49 | angleMatrix(end) = [];
50 | 
51 | % generate the points x and y
52 | for k = 1:numVert
53 |     x(k) = (radius + radius*rand(1)*radVar) * cos(angleMatrix(k) + angleSeparation*rand(1)*angVar);
54 |     y(k) = (radius + radius*rand(1)*radVar) * sin(angleMatrix(k) + angleSeparation*rand(1)*angVar);
55 | end
56 | 
57 | Polygon_vertex = [x y];
58 | 
59 | %reverse to clock wise
60 | Polygon_vertex = flipud(Polygon_vertex);
61 | 
62 | shifted_polygon_vertex = circshift(Polygon_vertex, -1);
63 | %M = [Polygon_vertex shifted_polygon_vertex];
64 | 
65 | end


--------------------------------------------------------------------------------
/images/path_example.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/irvingvasquez/ocpp/f0097829c51f0678cdd5304c838d15299a3c3469/images/path_example.png


--------------------------------------------------------------------------------
/isAConvexPolygon.m:
--------------------------------------------------------------------------------
 1 | % CONACYT
 2 | % Copyright (c) 2017, J. Irving Vasquez-Gomez,
 3 | % All rights reserved.
 4 | % 
 5 | % Redistribution and use in source and binary forms, with or without
 6 | % modification, are permitted provided that the following conditions are met:
 7 | %     * Redistributions of source code must retain the above copyright
 8 | %       notice, this list of conditions and the following disclaimer.
 9 | %     * Redistributions in binary form must reproduce the above copyright
10 | %       notice, this list of conditions and the following disclaimer in the
11 | %       documentation and/or other materials provided with the distribution.
12 | %     * Neither the name of the <organization> nor the
13 | %       names of its contributors may be used to endorse or promote products
14 | %       derived from this software without specific prior written permission.
15 | % 
16 | % THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 | % ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 | % WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 | % DISCLAIMED. IN NO EVENT SHALL J. Irving Vasquez-Gomez BE LIABLE FOR ANY
20 | % DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 | % (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 | % LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 | % ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 | % (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 | % SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 | 
27 | function isConvex = isAConvexPolygon(M)
28 |     [m, ~] = size(M);
29 |     sum = 0;
30 |     isConvex = true;
31 |     
32 |     for i = 1:m
33 |         j = mod(i,m)+1;
34 |         angle_i = pi - angleAP(M, i, j);
35 |         
36 |         if angle_i > pi || angle_i < 0
37 |             isConvex = false;
38 |             
39 |         end
40 |     end
41 | end
42 | 


--------------------------------------------------------------------------------
/lineSegmentIntersect.m:
--------------------------------------------------------------------------------
  1 | % Copyright (c) 2010, U. Murat Erdem 
  2 | % All rights reserved.
  3 | % 
  4 | % Redistribution and use in source and binary forms, with or without 
  5 | % modification, are permitted provided that the following conditions are 
  6 | % met:
  7 | % 
  8 | % * Redistributions of source code must retain the above copyright 
  9 | % notice, this list of conditions and the following disclaimer. 
 10 | % * Redistributions in binary form must reproduce the above copyright 
 11 | % notice, this list of conditions and the following disclaimer in 
 12 | % the documentation and/or other materials provided with the distribution
 13 | % 
 14 | % THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
 15 | % AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 
 16 | % IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 
 17 | % ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 
 18 | % LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 
 19 | % CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 
 20 | % SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 
 21 | % INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 
 22 | % CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 
 23 | % ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 
 24 | % POSSIBILITY OF SUCH DAMAGE.
 25 | 
 26 | function out = lineSegmentIntersect(XY1,XY2)
 27 | %LINESEGMENTINTERSECT Intersections of line segments.
 28 | %   OUT = LINESEGMENTINTERSECT(XY1,XY2) finds the 2D Cartesian Coordinates of
 29 | %   intersection points between the set of line segments given in XY1 and XY2.
 30 | %
 31 | %   XY1 and XY2 are N1x4 and N2x4 matrices. Rows correspond to line segments. 
 32 | %   Each row is of the form [x1 y1 x2 y2] where (x1,y1) is the start point and 
 33 | %   (x2,y2) is the end point of a line segment:
 34 | %
 35 | %                  Line Segment
 36 | %       o--------------------------------o
 37 | %       ^                                ^
 38 | %    (x1,y1)                          (x2,y2)
 39 | %
 40 | %   OUT is a structure with fields:
 41 | %
 42 | %   'intAdjacencyMatrix' : N1xN2 indicator matrix where the entry (i,j) is 1 if
 43 | %       line segments XY1(i,:) and XY2(j,:) intersect.
 44 | %
 45 | %   'intMatrixX' : N1xN2 matrix where the entry (i,j) is the X coordinate of the
 46 | %       intersection point between line segments XY1(i,:) and XY2(j,:).
 47 | %
 48 | %   'intMatrixY' : N1xN2 matrix where the entry (i,j) is the Y coordinate of the
 49 | %       intersection point between line segments XY1(i,:) and XY2(j,:).
 50 | %
 51 | %   'intNormalizedDistance1To2' : N1xN2 matrix where the (i,j) entry is the
 52 | %       normalized distance from the start point of line segment XY1(i,:) to the
 53 | %       intersection point with XY2(j,:).
 54 | %
 55 | %   'intNormalizedDistance2To1' : N1xN2 matrix where the (i,j) entry is the
 56 | %       normalized distance from the start point of line segment XY1(j,:) to the
 57 | %       intersection point with XY2(i,:).
 58 | %
 59 | %   'parAdjacencyMatrix' : N1xN2 indicator matrix where the (i,j) entry is 1 if
 60 | %       line segments XY1(i,:) and XY2(j,:) are parallel.
 61 | %
 62 | %   'coincAdjacencyMatrix' : N1xN2 indicator matrix where the (i,j) entry is 1 
 63 | %       if line segments XY1(i,:) and XY2(j,:) are coincident.
 64 | 
 65 | % Version: 1.00, April 03, 2010
 66 | % Version: 1.10, April 10, 2010
 67 | % Author:  U. Murat Erdem
 68 | 
 69 | % CHANGELOG:
 70 | %
 71 | % Ver. 1.00: 
 72 | %   -Initial release.
 73 | % 
 74 | % Ver. 1.10:
 75 | %   - Changed the input parameters. Now the function accepts two sets of line
 76 | %   segments. The intersection analysis is done between these sets and not in
 77 | %   the same set.
 78 | %   - Changed and added fields of the output. Now the analysis provides more
 79 | %   information about the intersections and line segments.
 80 | %   - Performance tweaks.
 81 | 
 82 | % I opted not to call this 'curve intersect' because it would be misleading
 83 | % unless you accept that curves are pairwise linear constructs.
 84 | % I tried to put emphasis on speed by vectorizing the code as much as possible.
 85 | % There should still be enough room to optimize the code but I left those out
 86 | % for the sake of clarity.
 87 | % The math behind is given in:
 88 | %   http://local.wasp.uwa.edu.au/~pbourke/geometry/lineline2d/
 89 | % If you really are interested in squeezing as much horse power as possible out
 90 | % of this code I would advise to remove the argument checks and tweak the
 91 | % creation of the OUT a little bit.
 92 | 
 93 | %%% Argument check.
 94 | %-------------------------------------------------------------------------------
 95 | 
 96 | validateattributes(XY1,{'numeric'},{'2d','finite'});
 97 | validateattributes(XY2,{'numeric'},{'2d','finite'});
 98 | 
 99 | [n_rows_1,n_cols_1] = size(XY1);
100 | [n_rows_2,n_cols_2] = size(XY2);
101 | 
102 | if n_cols_1 ~= 4 || n_cols_2 ~= 4
103 |     error('Arguments must be a Nx4 matrices.');
104 | end
105 | 
106 | %%% Prepare matrices for vectorized computation of line intersection points.
107 | %-------------------------------------------------------------------------------
108 | X1 = repmat(XY1(:,1),1,n_rows_2);
109 | X2 = repmat(XY1(:,3),1,n_rows_2);
110 | Y1 = repmat(XY1(:,2),1,n_rows_2);
111 | Y2 = repmat(XY1(:,4),1,n_rows_2);
112 | 
113 | XY2 = XY2';
114 | 
115 | X3 = repmat(XY2(1,:),n_rows_1,1);
116 | X4 = repmat(XY2(3,:),n_rows_1,1);
117 | Y3 = repmat(XY2(2,:),n_rows_1,1);
118 | Y4 = repmat(XY2(4,:),n_rows_1,1);
119 | 
120 | X4_X3 = (X4-X3);
121 | Y1_Y3 = (Y1-Y3);
122 | Y4_Y3 = (Y4-Y3);
123 | X1_X3 = (X1-X3);
124 | X2_X1 = (X2-X1);
125 | Y2_Y1 = (Y2-Y1);
126 | 
127 | numerator_a = X4_X3 .* Y1_Y3 - Y4_Y3 .* X1_X3;
128 | numerator_b = X2_X1 .* Y1_Y3 - Y2_Y1 .* X1_X3;
129 | denominator = Y4_Y3 .* X2_X1 - X4_X3 .* Y2_Y1;
130 | 
131 | u_a = numerator_a ./ denominator;
132 | u_b = numerator_b ./ denominator;
133 | 
134 | % Find the adjacency matrix A of intersecting lines.
135 | INT_X = X1+X2_X1.*u_a;
136 | INT_Y = Y1+Y2_Y1.*u_a;
137 | INT_B = (u_a >= 0) & (u_a <= 1) & (u_b >= 0) & (u_b <= 1);
138 | PAR_B = denominator == 0;
139 | COINC_B = (numerator_a == 0 & numerator_b == 0 & PAR_B);
140 | 
141 | 
142 | % Arrange output.
143 | out.intAdjacencyMatrix = INT_B;
144 | out.intMatrixX = INT_X .* INT_B;
145 | out.intMatrixY = INT_Y .* INT_B;
146 | out.intNormalizedDistance1To2 = u_a;
147 | out.intNormalizedDistance2To1 = u_b;
148 | out.parAdjacencyMatrix = PAR_B;
149 | out.coincAdjacencyMatrix= COINC_B;
150 | 
151 | end
152 | 
153 | 


--------------------------------------------------------------------------------
/loadPolygonCSV.m:
--------------------------------------------------------------------------------
 1 | % CONACYT
 2 | % Copyright (c) 2017, J. Irving Vasquez-Gomez,
 3 | % All rights reserved.
 4 | % 
 5 | % Redistribution and use in source and binary forms, with or without
 6 | % modification, are permitted provided that the following conditions are met:
 7 | %     * Redistributions of source code must retain the above copyright
 8 | %       notice, this list of conditions and the following disclaimer.
 9 | %     * Redistributions in binary form must reproduce the above copyright
10 | %       notice, this list of conditions and the following disclaimer in the
11 | %       documentation and/or other materials provided with the distribution.
12 | %     * Neither the name of the <organization> nor the
13 | %       names of its contributors may be used to endorse or promote products
14 | %       derived from this software without specific prior written permission.
15 | % 
16 | % THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 | % ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 | % WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 | % DISCLAIMED. IN NO EVENT SHALL J. Irving Vasquez-Gomez BE LIABLE FOR ANY
20 | % DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 | % (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 | % LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 | % ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 | % (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 | % SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 | 
27 | % Loads the polygon from a CSV file
28 | 
29 | function [Polygon, shifted_Polygon] = loadPolygonCSV(file)
30 |     Polygon = readmatrix(file);
31 |     shifted_Polygon = circshift(Polygon, -1);
32 | end


--------------------------------------------------------------------------------
/rotatePolygon.m:
--------------------------------------------------------------------------------
 1 | % Copyright (c) <year>, <copyright holder>
 2 | % All rights reserved.
 3 | % 
 4 | % Redistribution and use in source and binary forms, with or without
 5 | % modification, are permitted provided that the following conditions are met:
 6 | %     * Redistributions of source code must retain the above copyright
 7 | %       notice, this list of conditions and the following disclaimer.
 8 | %     * Redistributions in binary form must reproduce the above copyright
 9 | %       notice, this list of conditions and the following disclaimer in the
10 | %       documentation and/or other materials provided with the distribution.
11 | %     * Neither the name of the <organization> nor the
12 | %       names of its contributors may be used to endorse or promote products
13 | %       derived from this software without specific prior written permission.
14 | % 
15 | % THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
16 | % ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
17 | % WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
18 | % DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
19 | % DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
20 | % (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
21 | % LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
22 | % ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
23 | % (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
24 | % SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
25 | 
26 | 
27 | function M2 = rotatePolygon(M , theta)
28 | % Rotates a point in counter clock wise sense
29 | % M is a polygon in the form:
30 | % [px0 py0]
31 | % [px1 py1]
32 | % . . .
33 | % [pxn pyn]
34 | 
35 |     R = [cos(theta) -sin(theta); sin(theta) cos(theta)];
36 |     M2 = R * M';
37 | end


--------------------------------------------------------------------------------
/testOptimalCoverageRotCalip.m:
--------------------------------------------------------------------------------
  1 | % CONACYT
  2 | % Copyright (c) 2017, J. Irving Vasquez-Gomez,
  3 | % All rights reserved.
  4 | % 
  5 | % Redistribution and use in source and binary forms, with or without
  6 | % modification, are permitted provided that the following conditions are met:
  7 | %     * Redistributions of source code must retain the above copyright
  8 | %       notice, this list of conditions and the following disclaimer.
  9 | %     * Redistributions in binary form must reproduce the above copyright
 10 | %       notice, this list of conditions and the following disclaimer in the
 11 | %       documentation and/or other materials provided with the distribution.
 12 | %     * Neither the name of the <organization> nor the
 13 | %       names of its contributors may be used to endorse or promote products
 14 | %       derived from this software without specific prior written permission.
 15 | % 
 16 | % THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 17 | % ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 18 | % WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 19 | % DISCLAIMED. IN NO EVENT SHALL J. Irving Vasquez-Gomez BE LIABLE FOR ANY
 20 | % DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 21 | % (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 22 | % LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 23 | % ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 24 | % (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 25 | % SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 26 | 
 27 | % Program that test the method of optimal coverage path planning based on
 28 | % rotating calipers algorithm. 
 29 | 
 30 | 
 31 | % Configuration
 32 | n_vertices = 5;
 33 | polygon_radius = 100; %meters % experimentos en 100
 34 | rad_var = 1 ;%5
 35 | ang_var = 1 ;%1
 36 | dx = 20;
 37 | transl_spd = 10;
 38 | rot_spd = pi/4;
 39 | 
 40 | samplingx0 = -300;
 41 | samplingx1 = 300;
 42 | samplingy0 = -300;
 43 | samplingy1 = 300;
 44 | 
 45 | x_start = (samplingx1- samplingx0) * rand() + samplingx0;
 46 | y_start = (samplingy1- samplingy0) * rand() + samplingy0;
 47 | x_end = (samplingx1- samplingx0) * rand() + samplingx0;
 48 | y_end = (samplingy1- samplingy0) * rand() + samplingy0;
 49 | 
 50 | x_end = x_start
 51 | y_end = y_start
 52 | 
 53 | % get polygon
 54 | [M, Mshifted]= getConvexPolygon(n_vertices,polygon_radius,rad_var,ang_var);
 55 | 
 56 | %----- Optimal coverage path planning -----
 57 | tic;
 58 | % Compute Antipodal pairs
 59 | A = antipodalPoints(M)
 60 | [m, ~] = size(A);
 61 | 
 62 | % Graph polygon and antipodal points
 63 | %figure('Position',[10 100 500 500],'Renderer','zbuffer');
 64 | %axis equal; hold on;
 65 | %line([M(:,1)';Mshifted(:,1)'],[M(:,2)';Mshifted(:,2)'],'Color','k');
 66 | %title('Antipodal pairs');
 67 | %xlabel('East (x)'); ylabel('North (y)');
 68 | sz = 25; c = linspace(1,10,m);
 69 | %for i=1:m
 70 | %    scatter( M(A(i,1),1), M(A(i,1),2), sz, c(i), 'filled' );
 71 | %    scatter( M(A(i,2),1), M(A(i,2),2), sz, c(i), 'filled' );
 72 | %    line([M(A(i,1),1); M(A(i,2),1)],[M(A(i,1),2);M(A(i,2),2)],'Color',[rand() rand() rand()],'LineStyle','--');
 73 | %end
 74 | %hold off;
 75 | %%scatter(x_start, y_start, 25, 'filled');
 76 | %%scatter(x_end, y_end, 25, 'filled');
 77 | 
 78 | min_cost = Inf;
 79 | optimal_path = [];
 80 | best_antipodal_pair = 0;
 81 | 
 82 | for i=1:m 
 83 |     Path = bestPathForAntipodalPair(M, A(i,:), dx);
 84 |          
 85 |     %check if the path should be inverted
 86 |     FullPath1 = [x_start y_start; Path;  x_end y_end];
 87 |     FullPath2 = [x_start y_start; flipud(Path); x_end y_end];
 88 |     
 89 |     cost1 = timeCost2D(FullPath1, transl_spd, rot_spd, [x_start y_start 0]);
 90 |     
 91 |     cost2 =  timeCost2D(FullPath2, transl_spd, rot_spd, [x_start y_start 0]);
 92 |     
 93 |     if (cost1 < cost2)
 94 |        FullPath = FullPath1;
 95 |        Cost(i) = cost1;
 96 |     else
 97 |        FullPath = FullPath2;
 98 |        Cost(i) = cost2;
 99 |     end
100 |     
101 |     if Cost(i)<min_cost
102 |         min_cost = Cost(i);
103 |         optimal_path = FullPath;
104 |         best_antipodal_pair = i;
105 |     end
106 |         
107 | %--------- Draw the best path for an antipodal pair
108 | %     figure;
109 | %     axis equal;
110 | %     line([M(:,1)';Mshifted(:,1)'],[M(:,2)';Mshifted(:,2)'],'Color','k');
111 | %     title('Best path for an antipodal pair');
112 | %     ylabel('x(meters)');
113 | %     xlabel('y(meters)');
114 | %     hold on;
115 | %     scatter( M(A(i,1),1), M(A(i,1),2), sz, c(i), 'filled' );
116 | %     scatter( M(A(i,2),1), M(A(i,2),2), sz, c(i), 'filled' );
117 | %     plot(FullPath(:,1), FullPath(:,2));
118 | %     txt1 = ['cost = ', num2str(Cost(i))];
119 | %     text(x_start,y_start,txt1);
120 | %     hold off;
121 | end
122 | 
123 | tiempo_vasq = toc
124 | 
125 | figure('Position',[10+500 100 500 500]);
126 | axis equal;
127 | line([M(:,1)';Mshifted(:,1)'],[M(:,2)';Mshifted(:,2)'],'Color','k');
128 | title('Vasquez');
129 | xlabel('East (x)');
130 | ylabel('North (y)');
131 | hold on;
132 | i = best_antipodal_pair;
133 | scatter( M(A(i,1),1), M(A(i,1),2), sz, c(i), 'filled' );
134 | scatter( M(A(i,2),1), M(A(i,2),2), sz, c(i), 'filled' );
135 | %line([M(A(i,1),1); M(A(i,2),1)],[M(A(i,1),2);M(A(i,2),2)],'Color',[rand() rand() rand()],'LineStyle','--');
136 | 
137 | %display('Vasquez cost')
138 | %timeCost2D(optimal_path, transl_spd, rot_spd, [x_start y_start 0])
139 | plot(optimal_path(:,1), optimal_path(:,2), '-o');
140 | scatter(x_start, y_start, 25, 'filled');
141 | scatter(x_end, y_end, 25, 'filled');
142 | txt1 = ['cost = ', num2str(min_cost)];
143 | text(x_start,y_start,txt1);
144 | hold off;
145 |     
146 |     
147 | % ------- Compare with Torres --------
148 | tic
149 | [PathTorres, PathTorres_counter, r_torres] = torres16(M, dx);
150 | %check if the path should be inverted
151 | FullPath1 = [x_start y_start; PathTorres; x_end y_end];
152 | FullPath2 = [x_start y_start; flipud(PathTorres); x_end y_end];
153 | FullPath3 = [x_start y_start; PathTorres_counter; x_end y_end];
154 | FullPath4 = [x_start y_start; flipud(PathTorres_counter); x_end y_end];
155 | 
156 | cost1 = timeCost2D(FullPath1, transl_spd, rot_spd, [x_start y_start 0]);
157 | cost2 = timeCost2D(FullPath2, transl_spd, rot_spd, [x_start y_start 0]);
158 | cost3 = timeCost2D(FullPath3, transl_spd, rot_spd, [x_start y_start 0]);
159 | cost4 = timeCost2D(FullPath4, transl_spd, rot_spd, [x_start y_start 0]);
160 | 
161 | torres_cost = cost1;
162 | FullPathTorres = FullPath1;
163 | 
164 | if (cost2 < torres_cost)
165 |     FullPathTorres = FullPath2;
166 |     torres_cost = cost2;
167 | end
168 | 
169 | if (cost3 < torres_cost)
170 |     FullPathTorres = FullPath3;
171 |     torres_cost = cost3;
172 | end
173 | 
174 | if (cost4 < torres_cost)
175 |     FullPathTorres = FullPath4;
176 |     torres_cost = cost4;
177 | end
178 | 
179 | time_torr = toc
180 | 
181 | %FullPathTorres = [x_start y_start; PathTorres; x_end y_end];
182 | %display('Torres cost');
183 | %timeCost2D(FullPathTorres, transl_spd, rot_spd, [x_start y_start 0])
184 | 
185 | figure('Position',[10+1000 100 500 500]);
186 | axis equal;
187 | line([M(:,1)';Mshifted(:,1)'],[M(:,2)';Mshifted(:,2)'],'Color','k');
188 | title('Torres 16 Path');
189 | xlabel('East (x)');
190 | ylabel('North (y)');
191 | hold on;
192 | plot(FullPathTorres(:,1), FullPathTorres(:,2), '-o');
193 | scatter(x_start, y_start, 25, 'filled');
194 | scatter(x_end, y_end, 25, 'filled');
195 | txt1 = ['cost = ', num2str(torres_cost)];
196 | text(x_start,y_start,txt1);
197 | hold off;


--------------------------------------------------------------------------------
/testRCPP.m:
--------------------------------------------------------------------------------
 1 | %% Example Title
 2 | % Summary of example objective
 3 | 
 4 | %% Section 1 Title
 5 | % Description of first code block
 6 | polygon_file = "data\target_rcpp_room.txt";
 7 | starting_point_file = "data\p_e_rcpp.txt";
 8 | ending_point_file = "data\p_x_rcpp.txt";
 9 | dx = 20;
10 | 
11 | [poly, poly_s] = loadPolygonCSV(polygon_file);
12 | s_point = readmatrix(starting_point_file);
13 | e_point = readmatrix(ending_point_file);
14 | %home_point
15 | 
16 | x_start = s_point(1);
17 | y_start = s_point(2);
18 | x_end = e_point(1);
19 | y_end = e_point(2);
20 | 
21 | tic;
22 | [optimal_path, min_cost] = RCPP(poly, dx, s_point, e_point, 40.0, 0.7);
23 | RCPP_time = toc;
24 | RCPP_time
25 | 
26 | 
27 | %% Section 2 Title
28 | % Description of second code block
29 | %line([poly(:,1)';poly_s(:,1)'],[poly(:,2)';poly_s(:,2)'],'Color','k');
30 | 
31 | figure('Position',[10+500 100 500 500]);
32 | axis equal;
33 | line([poly(:,1)';poly_s(:,1)'],[poly(:,2)';poly_s(:,2)'],'Color','k');
34 | title('Vasquez');
35 | xlabel('East (x)');
36 | ylabel('North (y)');
37 | hold on;
38 | %i = best_antipodal_pair;
39 | %scatter( poly(A(i,1),1), poly(A(i,1),2), sz, c(i), 'filled' );
40 | %scatter( poly(A(i,2),1), poly(A(i,2),2), sz, c(i), 'filled' );
41 | %line([poly(A(i,1),1); poly(A(i,2),1)],[poly(A(i,1),2);poly(A(i,2),2)],'Color',[rand() rand() rand()],'LineStyle','--');
42 | 
43 | %display('Vasquez cost')
44 | %timeCost2D(optimal_path, transl_spd, rot_spd, [x_start y_start 0])
45 | plot(optimal_path(:,1), optimal_path(:,2), '-o');
46 | scatter(x_start, y_start, 25, 'filled');
47 | scatter(x_end, y_end, 25, 'filled');
48 | txt1 = ['cost = ', num2str(min_cost)];
49 | text(x_start,y_start,txt1);
50 | hold off;
51 | 
52 | path_file = "C:\Users\vagop\projects\sweep_cpp\out\path.txt";
53 | writematrix(optimal_path, path_file);
54 | 
55 | path_file = "C:\Users\vagop\projects\sweep_cpp\out\lenght.txt";
56 | writematrix(min_cost, path_file);
57 | 


--------------------------------------------------------------------------------
/timeCost2D.m:
--------------------------------------------------------------------------------
 1 | % Copyright (c) <year>, <copyright holder>
 2 | % All rights reserved.
 3 | % 
 4 | % Redistribution and use in source and binary forms, with or without
 5 | % modification, are permitted provided that the following conditions are met:
 6 | %     * Redistributions of source code must retain the above copyright
 7 | %       notice, this list of conditions and the following disclaimer.
 8 | %     * Redistributions in binary form must reproduce the above copyright
 9 | %       notice, this list of conditions and the following disclaimer in the
10 | %       documentation and/or other materials provided with the distribution.
11 | %     * Neither the name of the <organization> nor the
12 | %       names of its contributors may be used to endorse or promote products
13 | %       derived from this software without specific prior written permission.
14 | % 
15 | % THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
16 | % ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
17 | % WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
18 | % DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
19 | % DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
20 | % (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
21 | % LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
22 | % ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
23 | % (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
24 | % SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
25 | 
26 | 
27 | 
28 | function [cost] = timeCost2D(Path, transl_spd, rot_spd, q_current)
29 | % transl_spd in mts per second
30 | % rot_spd in radians per second
31 |     cost = 0;
32 |     q_t = q_current;
33 |     [n, d] = size(Path);
34 |     
35 |     for i=1:n
36 |         q_goal = Path(i,:);
37 |         
38 |         if q_t(1:d) ~= q_goal
39 |             % rotate
40 |             theta_g = atan2( q_goal(2)-q_t(2), q_goal(1)-q_t(1));
41 |             % find the shortest rotation
42 |             theta_delta = minRotation(q_t(3),theta_g); 
43 |             if(abs(theta_delta)>pi)
44 |                 display('wtf!');
45 |                 pause;
46 |             end
47 |             time = abs(theta_delta)/rot_spd;
48 |             cost = cost + time;
49 |             
50 |             %translate
51 |             dist = norm(q_goal-q_t(1:d));
52 |             time = dist / transl_spd;
53 |             cost = cost + time;
54 |             
55 |             %update
56 |             q_t = [q_goal theta_g];
57 |         end    
58 |     end
59 |     
60 |     %finally rotate to the initial orientation
61 |     theta_g = q_current(3);
62 |     theta_delta = minRotation(q_t(3),theta_g);            
63 |     time = abs(theta_delta)/rot_spd;
64 |     cost = cost + time;
65 | end
66 | 
67 | function theta_dif = minRotation(theta, theta_goal)
68 |     if theta_goal > 0
69 |         if theta > 0
70 |             theta_dif = theta_goal-theta;
71 |         else
72 |             theta_dif = minR(theta_goal-theta, theta_goal-(2*pi+theta));
73 |         end
74 |     else
75 |         if theta > 0
76 |             theta_dif = minR(theta_goal-theta, (2*pi+theta_goal)-theta);
77 |         else
78 |             theta_dif = theta_goal - theta;
79 |         end
80 |     end
81 | end
82 | 
83 | function c = minR(a,b)
84 |     if abs(a) < abs(b)
85 |         c = a;
86 |     else
87 |         c = b;
88 |     end
89 | end


--------------------------------------------------------------------------------
/torres16.m:
--------------------------------------------------------------------------------
 1 | % Copyright (c) <year>, <copyright holder>
 2 | % All rights reserved.
 3 | % 
 4 | % Redistribution and use in source and binary forms, with or without
 5 | % modification, are permitted provided that the following conditions are met:
 6 | %     * Redistributions of source code must retain the above copyright
 7 | %       notice, this list of conditions and the following disclaimer.
 8 | %     * Redistributions in binary form must reproduce the above copyright
 9 | %       notice, this list of conditions and the following disclaimer in the
10 | %       documentation and/or other materials provided with the distribution.
11 | %     * Neither the name of the <organization> nor the
12 | %       names of its contributors may be used to endorse or promote products
13 | %       derived from this software without specific prior written permission.
14 | % 
15 | % THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
16 | % ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
17 | % WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
18 | % DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
19 | % DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
20 | % (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
21 | % LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
22 | % ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
23 | % (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
24 | % SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
25 | 
26 | 
27 | % Torres et al 16
28 | % flight lines rotattion angle
29 | % V: vertices of the polygon
30 | % dx: distance between lines
31 | 
32 | function [Path_clock, Path_counter_clock, flra] = torres16(V, dx)
33 |     [n,~] = size(V);
34 |     optimal_dist = Inf;
35 |     
36 |     %for all edges in the polygon
37 |     for i=1:n;
38 |         max_dist_edge = 0;
39 |         %for all vertex in the polygon
40 |         for j=1:n;
41 |             if distPoint2Line(V(i,:),V(increment(i,n),:),V(j,:))>max_dist_edge
42 |                 max_dist_edge = distPoint2Line(V(i,:),V(increment(i,n),:),V(j,:));
43 |                 opposed_vertex = j;
44 |             end
45 |         end
46 |         
47 |         if max_dist_edge < optimal_dist
48 |             optimal_dist = max_dist_edge;
49 |             angle = slopeAngle(V(i,:), V(increment(i,n),:));
50 |         end
51 |     end
52 |     
53 |     flra = angle;
54 |     alpha = angle-pi/2; % R rotation is with respect of the east; alpha rotation is with respect of the north
55 |     Pstart = V;
56 |     Pend = circshift(V, -1);
57 |     PsR = rotatePolygon(Pstart, -alpha);
58 |     PsR = PsR';
59 |     PeR = rotatePolygon(Pend, -alpha);
60 |     PeR = PeR';
61 | 
62 |     % clock wise path
63 |     [PathRotated] = getPathMR([PsR PeR], dx, 1);
64 |     Path_clock = rotatePolygon(PathRotated, alpha);
65 |     Path_clock = Path_clock';
66 |     
67 |     % counter clock wise path
68 |     [PathRotated] = getPathMR([PsR PeR], dx, -1);
69 |     Path_counter_clock = rotatePolygon(PathRotated, alpha);
70 |     Path_counter_clock = Path_counter_clock';
71 | end
72 | 
73 | function i_next = increment(i,n)
74 |     i_next = mod(i,n)+1;
75 | end
76 | 
77 | function alpha = slopeAngle(p1, p2)
78 |     alpha = atan2(p2(2)-p1(2),p2(1)-p1(1));
79 | end


--------------------------------------------------------------------------------
/torres16simple.m:
--------------------------------------------------------------------------------
 1 | % Copyright (c) <year>, <copyright holder>
 2 | % All rights reserved.
 3 | % 
 4 | % Redistribution and use in source and binary forms, with or without
 5 | % modification, are permitted provided that the following conditions are met:
 6 | %     * Redistributions of source code must retain the above copyright
 7 | %       notice, this list of conditions and the following disclaimer.
 8 | %     * Redistributions in binary form must reproduce the above copyright
 9 | %       notice, this list of conditions and the following disclaimer in the
10 | %       documentation and/or other materials provided with the distribution.
11 | %     * Neither the name of the <organization> nor the
12 | %       names of its contributors may be used to endorse or promote products
13 | %       derived from this software without specific prior written permission.
14 | % 
15 | % THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
16 | % ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
17 | % WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
18 | % DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
19 | % DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
20 | % (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
21 | % LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
22 | % ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
23 | % (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
24 | % SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
25 | 
26 | 
27 | % Torres et al 16
28 | % flight lines rotattion angle
29 | % V: vertices of the polygon
30 | % dx: distance between lines
31 | 
32 | function [Path, flra] = torres16simple(V, dx)
33 |     [n,~] = size(V);
34 |     optimal_dist = Inf;
35 |     
36 |     %for all edges in the polygon
37 |     for i=1:n;
38 |         max_dist_edge = 0;
39 |         %for all vertex in the polygon
40 |         for j=1:n;
41 |             if distPoint2Line(V(i,:),V(increment(i,n),:),V(j,:))>max_dist_edge
42 |                 max_dist_edge = distPoint2Line(V(i,:),V(increment(i,n),:),V(j,:));
43 |                 opposed_vertex = j;
44 |             end
45 |         end
46 |         
47 |         if max_dist_edge < optimal_dist
48 |             optimal_dist = max_dist_edge;
49 |             angle = slopeAngle(V(i,:), V(increment(i,n),:));
50 |         end
51 |     end
52 |     
53 |     flra = angle;
54 |     alpha = angle-pi/2; % R rotation is with respect of the east; alpha rotation is with respect of the north
55 |     Pstart = V;
56 |     Pend = circshift(V, -1);
57 |     PsR = rotatePolygon(Pstart, -alpha);
58 |     PsR = PsR';
59 |     PeR = rotatePolygon(Pend, -alpha);
60 |     PeR = PeR';
61 | 
62 |     [PathRotated, ~] = getPathMR([PsR PeR], dx, 1);
63 |     Path = rotatePolygon(PathRotated, alpha);
64 |        
65 |     Path = Path';
66 | end
67 | 
68 | function i_next = increment(i,n)
69 |     i_next = mod(i,n)+1;
70 | end
71 | 
72 | function alpha = slopeAngle(p1, p2)
73 |     alpha = atan2(p2(2)-p1(2),p2(1)-p1(1));
74 | end


--------------------------------------------------------------------------------