one shot and other fixes
[swamp-bikeopera:code.git] / lib / poly.lua
1 -- Swamp Bike Opera embedded system for Kaffe Matthews
2 -- Copyright (C) 2012 Wolfgang Hauptfleisch, Dave Griffiths
3 --
4 -- This program is free software: you can redistribute it and/or modify
5 -- it under the terms of the GNU General Public License as published by
6 -- the Free Software Foundation, either version 3 of the License, or
7 -- (at your option) any later version.
8 --
9 -- This program is distributed in the hope that it will be useful,
10 -- but WITHOUT ANY WARRANTY; without even the implied warranty of
11 -- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
12 -- GNU General Public License for more details.
13 --
14 -- You should have received a copy of the GNU General Public License
15 -- along with this program.  If not, see <http://www.gnu.org/licenses/>.
16
17 module("poly", package.seeall)
18
19 ---
20 -- define if point is inside a polygon
21 function is_in_polygon(vertices, x, y)
22         local intersectionCount = 0
23
24         local x0 = vertices[#vertices].lat - x
25         local y0 = vertices[#vertices].lng - y
26
27         for i=1,#vertices do
28                 local x1 = vertices[i].lat - x
29                 local y1 = vertices[i].lng - y
30
31                 if y0 > 0 and y1 <= 0 and x1 * y0 > y1 * x0 then
32                         intersectionCount = intersectionCount + 1
33                 end
34
35                 if y1 > 0 and y0 <= 0 and x0 * y1 > y0 * x1 then
36                         intersectionCount = intersectionCount + 1
37                 end
38
39                 x0 = x1
40                 y0 = y1
41         end
42
43         return (intersectionCount % 2) == 1
44 end
45
46
47
48 function blend_polygon(vertsa, vertsb, t)
49     if (#vertsa ~= #vertsb) then
50        return false
51     end
52
53     local ret={}
54     for i=1,#vertsa do
55         table.insert(ret,lerp(vertsa[i],vertsb[i],t))
56     end
57     return ret
58 end
59
60 -- is the polygon within the specified radius, in km
61 function distance_to_polygon_points(vertices, x, y)
62     local dist=99999999;
63     for i=1,#vertices do
64         local d=distance_km(vertices[i].lat,
65                             vertices[i].lng,
66                             x,y);
67          if d<dist then
68              dist=d
69          end
70     end
71     return dist
72 end
73
74 function distance_to_polygon(vertices, x, y)
75         local dist=99999999
76         local x0 = vertices[#vertices].lat
77         local y0 = vertices[#vertices].lng
78         for i=1,#vertices do
79                 local x1 = vertices[i].lat
80                 local y1 = vertices[i].lng
81                 local d=distance_to_line(x,y,x0,y0,x1,y1)
82                 if (d<dist) then
83                    dist=d
84                 end
85                 x0 = x1
86                 y0 = y1
87         end
88         return dist
89 end
90
91 -- is the polygon within the specified distance, in cartesian lat/lng distance!
92 function is_close_polygon(vertices, x, y, radius)
93     -- in case polygon is larger than radius!
94     if (is_in_polygon(vertices,x,y)) then
95         return true
96     end
97
98     if (distance_to_polygon(vertices,x,y)<radius) then
99         return true
100     end
101     return false
102 end
103
104
105 -- are the points of the polygon within the specified radius, in km
106 function is_close_polygon_km(vertices, x, y, radius)
107     -- in case polygon is larger than radius!
108     if (is_in_polygon(vertices,x,y)) then
109         return true
110     end
111
112     for i=1,#vertices do
113         local d=distance_km(vertices[i].lat,
114                             vertices[i].lng,
115                            x,y);
116         if d<radius then
117             return true
118         end
119     end
120     return false
121 end
122
123 function distance_km(lat1,lon1,lat2,lon2)
124     local R = 6371; -- km
125     local la1=lat1*math.pi/180
126     local lo1=lon1*math.pi/180
127     local la2=lat2*math.pi/180
128     local lo2=lon2*math.pi/180
129     return math.acos(math.sin(la1)*math.sin(la2) +
130            math.cos(la1)*math.cos(la2) *
131            math.cos(lo2-lo1)) * R;
132 end
133
134 function distance_to_line(cx,cy,ax,ay,bx,by)
135    local r_numerator = (cx-ax)*(bx-ax) + (cy-ay)*(by-ay);
136    local r_denomenator = (bx-ax)*(bx-ax) + (by-ay)*(by-ay);
137    local r = r_numerator / r_denomenator;
138
139    if ( (r >= 0) and (r <= 1) ) then
140        local s =  ((ay-cy)*(bx-ax)-(ax-cx)*(by-ay) ) / r_denomenator;
141        return math.abs(s)*math.sqrt(r_denomenator);
142    else
143         local dist1 = (cx-ax)*(cx-ax) + (cy-ay)*(cy-ay);
144         local dist2 = (cx-bx)*(cx-bx) + (cy-by)*(cy-by);
145         if (dist1 < dist2) then
146             return math.sqrt(dist1);
147         else
148             return math.sqrt(dist2);
149         end
150     end
151 end
152
153 ---
154 -- calculate the centre of a polygon
155 function centroid(polygon)
156   local xsum = 0
157   local ysum = 0
158
159 for i, p in ipairs(polygon) do
160     xsum = xsum + p.x
161     ysum = ysum + p.y
162 end
163
164 local k = table.getn(polygon)
165 local cx = xsum / k
166 local cy = ysum / k
167 return cx, cy
168 end
169
170 -- calculate the distance between two coordinates
171 function distance(a, b)
172     local d = math.sqrt(math.pow((b.lat - a.lat), 2) +
173                         math.pow((b.lng - a.lng), 2))
174     return d
175 end
176
177 function length(x,y)
178     return math.sqrt(math.pow(x, 2) +
179                      math.pow(y, 2))
180 end
181
182 function length(a)
183     return math.sqrt(math.pow(a.lat, 2) +
184                      math.pow(a.lng, 2))
185 end
186
187 function normalise(a)
188     local len=length(a)
189     return {lat=a.lat/len,
190             lng=a.lng/len}
191 end
192
193 function direction(a, b)
194     local d={
195         lat=b.lat-a.lat,
196         lng=b.lng-a.lng
197     }
198     local d=normalise(d)
199     local angle = math.atan2(d.lng, d.lat) * 180 / math.pi;
200     return angle
201 end
202
203 function angle(a)
204     local d=normalise(a)
205     local angle = math.atan2(d.lng, d.lat) * 180 / math.pi;
206     return angle
207 end
208
209 function lerp(a,b,t)
210     return {lat=(1-t)*a.lat+t*b.lat,
211             lng=(1-t)*a.lng+t*b.lng}
212 end
213
214 function dot(a,b)
215     return a.lng*b.lng + a.lat*b.lat
216 end
217
218 ---
219 -- We can shift(move) a polygon into 4 directions
220 --
221 function polygon_shift(polygon, towards, offset)
222
223 local offset = offset or 1
224 local new_polygon = {}
225
226 for i, point in ipairs(polygon) do
227
228    if towards == "east" then
229       local x = point.x + offset
230       table.insert(new_polygon, { x = x, y = point.y } )
231    elseif towards == "north" then
232       local y = y + offset
233       table.insert(new_polygon, { x = point.x, y = y } )
234    elseif towards == "west" then
235       local x = point.x - offset
236       table.insert(new_polygon, { x = x, y = point.y } )
237    elseif towards == "south" then
238       local y = y - offset
239       table.insert(new_polygon, { x = point.x, y = y } )
240    end
241
242 end
243
244 return new_polygon
245 end
246
247 function test()
248     local a={lat=0, lng=1}
249     local b={lat=0, lng=0}
250     print(direction(a, b))
251
252 end
253
254 test()