1 //:
2 // \file
3 #include "bkml_write.h"
4 #include <cstdio>
5 #include <iomanip>
6 #include <iostream>
7 #include <utility>
8 #ifdef _MSC_VER
9 #  include "vcl_msvc_warnings.h"
10 #endif
11 
rgb_color_to_hex_color(int alpha,int rNum,int gNum,int bNum)12 std::string rgb_color_to_hex_color(int alpha, int rNum, int gNum, int bNum)
13 {
14   std::string result;
15   char a[255];
16   std::snprintf(a, 255, "%.2x", alpha);
17   result.append(a );
18   char b[255];
19   std::snprintf(b, 255, "%.2x", bNum);
20   result.append(b );
21   char g[255];
22   std::snprintf(g, 255, "%.2x", gNum);
23   result.append(g );
24   char r[255];
25   std::snprintf(r, 255, "%.2x", rNum);
26   result.append(r );
27   return result;
28 }
29 
30 //: Write KML header and open document tag
open_document(std::ofstream & str)31 void bkml_write::open_document(std::ofstream& str)
32 {
33   str << "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n"
34       << "<kml xmlns=\"http://www.opengis.net/kml/2.2\">\n<Document>\n";
35   return;
36 }
37 
38 //: end document tag
close_document(std::ofstream & str)39 void bkml_write::close_document(std::ofstream& str)
40 {
41   str << "</Document>\n</kml>\n";
42 }
43 
44 //: Write a box
write_box(std::ofstream & ofs,const std::string & name,const std::string & description,vnl_double_2 ul,vnl_double_2 ur,vnl_double_2 ll,vnl_double_2 lr)45 void bkml_write::write_box(std::ofstream &ofs, const std::string& name, const std::string& description, vnl_double_2 ul, vnl_double_2 ur, vnl_double_2 ll, vnl_double_2 lr)
46 {
47   ofs.precision(8);
48   ofs << "<Placemark>\n"
49       << "  <name>" << name << "</name>\n"
50       << "  <description>" << description << "</description>\n"
51       << "  <Style>\n"
52       << "    <PolyStyle>\n"
53       << "      <colorMode>Random</colorMode>\n"
54       << "      <color>ffffffff</color>\n"
55       << "      <fill>0</fill>\n"
56       << "    </PolyStyle>\n"
57       << "    <LineStyle>\n"
58       << "      <colorMode>Random</colorMode>\n"
59       << "      <color>ffffffff</color>\n"
60       << "      <width>3</width>\n"
61       << "    </LineStyle>\n"
62       << "  </Style>\n"
63       << "  <Polygon>\n"
64       << "   <tessellate>1</tessellate>\n"
65       << "   <outerBoundaryIs>\n"
66       << "     <LinearRing>\n"
67       << "       <coordinates>\n"
68       << "          "
69       << std::setprecision(12) << ul[1] << ',' << std::setprecision(12) << ul[0] << ",0  "
70       << std::setprecision(12) << ur[1] << ',' << std::setprecision(12) << ur[0] << ",0  "
71       << std::setprecision(12) << lr[1] << ',' << std::setprecision(12) << lr[0] << ",0  "
72       << std::setprecision(12) << ll[1] << ',' << std::setprecision(12) << ll[0] << ",0  "
73       << std::setprecision(12) << ul[1] << ',' << std::setprecision(12) << ul[0] << ",0\n"
74       << "       </coordinates>\n"
75       << "     </LinearRing>\n"
76       << "   </outerBoundaryIs>\n"
77       << "  </Polygon>\n"
78       << "</Placemark>\n" << std::endl;
79 }
80 
81 //: kml requires lon, lat, elev in polygon definition
write_box(std::ofstream & ofs,std::string name,std::string description,vgl_box_2d<double> bbox)82 void bkml_write::write_box(std::ofstream &ofs, std::string name, std::string description, vgl_box_2d<double> bbox)
83 {
84   // in ul x is lat y is lon, in vgl bbox x is lon, y is lat so reverse
85   vnl_double_2 ul(bbox.max_y(), bbox.min_x());
86   vnl_double_2 ur(bbox.max_y(), bbox.max_x());
87   vnl_double_2 ll(bbox.min_y(), bbox.min_x());
88   vnl_double_2 lr(bbox.min_y(), bbox.max_x());
89   bkml_write::write_box(ofs, std::move(name), std::move(description), ul, ur, ll, lr);
90 }
91 
92 //: Write a box with color
write_box(std::ofstream & ofs,const std::string & name,const std::string & description,vnl_double_2 ul,vnl_double_2 ur,vnl_double_2 ll,vnl_double_2 lr,const std::string & hex_color,unsigned const & fill)93 void bkml_write::write_box(std::ofstream &ofs, const std::string& name, const std::string& description, vnl_double_2 ul, vnl_double_2 ur, vnl_double_2 ll, vnl_double_2 lr, const std::string& hex_color, unsigned const& fill)
94 {
95   ofs << "<Placemark>\n"
96       << "  <name>" << name << "</name>\n"
97       << "  <description>" << description << "</description>\n"
98       << "  <Style>\n"
99       << "    <PolyStyle>\n"
100       << "      <colorMode>normal</colorMode>\n"
101       << "      <color>" << hex_color << "</color>\n"
102       << "      <fill>" << fill << "</fill>\n"
103       << "    </PolyStyle>\n"
104       << "    <LineStyle>\n"
105       << "      <colorMode>normal</colorMode>\n"
106       << "      <color>" << hex_color << "</color>\n"
107       << "      <width>3</width>\n"
108       << "    </LineStyle>\n"
109       << "  </Style>\n"
110       << "  <Polygon>\n"
111       << "   <tessellate>1</tessellate>\n"
112       << "   <outerBoundaryIs>\n"
113       << "     <LinearRing>\n"
114       << "       <coordinates>\n"
115       << "          "
116       << std::setprecision(12) << ul[1] << ',' << std::setprecision(12) << ul[0] << ",0  "
117       << std::setprecision(12) << ur[1] << ',' << std::setprecision(12) << ur[0] << ",0  "
118       << std::setprecision(12) << lr[1] << ',' << std::setprecision(12) << lr[0] << ",0  "
119       << std::setprecision(12) << ll[1] << ',' << std::setprecision(12) << ll[0] << ",0  "
120       << std::setprecision(12) << ul[1] << ',' << std::setprecision(12) << ul[0] << ",0\n"
121       << "       </coordinates>\n"
122       << "     </LinearRing>\n"
123       << "   </outerBoundaryIs>\n"
124       << "  </Polygon>\n"
125       << "</Placemark>\n" << std::endl;
126 }
127 
128 // write a box with color
write_box(std::ofstream & ofs,std::string name,std::string description,vnl_double_2 ul,vnl_double_2 ur,vnl_double_2 ll,vnl_double_2 lr,unsigned char const & r,unsigned char const & g,unsigned char const & b,unsigned char const & a,unsigned const & fill)129 void bkml_write::write_box(std::ofstream &ofs, std::string name, std::string description, vnl_double_2 ul, vnl_double_2 ur, vnl_double_2 ll, vnl_double_2 lr,
130                            unsigned char const& r, unsigned char const& g, unsigned char const& b, unsigned char const&a, unsigned const& fill)
131 {
132   std::string hex_color = rgb_color_to_hex_color((int)a, (int)r, (int)g, (int)b);
133   bkml_write::write_box(ofs, std::move(name), std::move(description), ul, ur, ll, lr, hex_color, fill);
134 }
135 
136 // write a polygon with color (only outerBoundary)
write_polygon(std::ofstream & ofs,vgl_polygon<double> const & poly,std::string const & name,std::string const & description,double const & scale,double const & line_width,double const & alpha,unsigned char const & r,unsigned char const & g,unsigned char const & b)137 void bkml_write::write_polygon(std::ofstream& ofs, vgl_polygon<double> const& poly,
138                                std::string const& name,
139                                std::string const& description,
140                                double const& scale,  double const& line_width, double const& alpha,
141                                unsigned char const& r, unsigned char const& g, unsigned char const& b)
142 {
143   // obtain line color
144   std::string line_color = rgb_color_to_hex_color(1, (int)r, (int)g, (int)b);
145 
146   // obtain polygon color
147   int alpha_int = (int)(alpha*255);
148   std::string poly_color = rgb_color_to_hex_color(alpha_int, (int)r, (int)g, (int)b);
149 
150   unsigned num_sheet = poly.num_sheets();
151   for (unsigned s_idx = 0; s_idx < num_sheet; s_idx++) {
152     std::vector<vgl_point_2d<double> > verts = poly[s_idx];
153     if (verts.empty())
154       continue;
155     ofs << "<Placemark>\n"
156         << "  <name>" << name << "</name>\n";
157     if (description.compare("") != 0)
158       ofs << "  <description>" << description << "</description>\n";
159     ofs << "  <Style>\n"
160         << "    <LabelStyle> <scale>" << scale << "</scale> </LabelStyle>\n"
161         << "    <LineStyle> <color>" << line_color << "</color> <width>" << line_width << "</width> </LineStyle>\n"
162         << "    <PolyStyle>\n"
163         << "      <color>" << poly_color << "</color>\n"
164         << "      <fill>1</fill>\n"
165         << "      <outline>0</outline>\n"
166         << "    </PolyStyle>\n"
167         << "  </Style>\n";
168     ofs << "  <Polygon>\n"
169         << "    <tessellate>1</tessellate>\n"
170         << "    <outerBoundaryIs>\n"
171         << "      <LinearRing>\n"
172         << "        <coordinates>\n";
173     for (auto & vert : verts)
174       ofs << "          " << std::setprecision(12) << vert.x() << ',' << std::setprecision(12) << vert.y() << ",0\n";
175     ofs << "          " << verts[0].x() << ',' << verts[0].y() << ",0\n";
176     ofs << "        </coordinates>\n"
177         << "      </LinearRing>\n"
178         << "    </outerBoundaryIs>\n"
179         << "  </Polygon>\n"
180         << "</Placemark>\n";
181   }
182 }
183 
184 //: write a path with color and line width
write_path(std::ofstream & ofs,std::vector<vgl_point_2d<double>> path,std::string const & name,std::string const & description,double const & scale,double const & line_width,double const & alpha,unsigned char const & r,unsigned char const & g,unsigned char const & b)185 void bkml_write::write_path(std::ofstream& ofs, std::vector<vgl_point_2d<double> > path,
186                          std::string const& name,
187                          std::string const& description,
188                          double const& scale,
189                          double const& line_width,
190                          double const& alpha,
191                          unsigned char const& r,
192                          unsigned char const& g,
193                          unsigned char const& b)
194 {
195   // obtain line color
196   int alpha_int = (int)(alpha*255);
197   std::string line_color = rgb_color_to_hex_color(alpha_int, (int)r, (int)g, (int)b);
198   if (path.empty())
199     return;
200   ofs << "<Placemark>\n"
201       << "  <name>" << name << "</name>\n";
202   if (description.compare("") != 0)
203     ofs << "  <description>" << description << "</description>\n";
204   ofs << "  <Style>\n"
205       << "    <LabelStyle> <scale>" << scale << "</scale> </LabelStyle>\n"
206       << "    <LineStyle> <color>" << line_color << "</color> <width>" << line_width << "</width> </LineStyle>\n"
207       << "  </Style>\n";
208   ofs << "  <LineString>\n"
209       << "    <tessellate>1</tessellate>\n"
210       << "      <coordinates>\n        ";
211   for (auto & vit : path)
212     ofs << std::setprecision(12) << vit.x() << ',' << std::setprecision(12) << vit.y() << ",0 ";
213   ofs << "\n      </coordinates>\n"
214       << "  </LineString>\n"
215       << "</Placemark>\n";
216 }
217 
218 //: put a pin at the given location
write_location(std::ofstream & ofs,const std::string & name,const std::string & description,double lat,double lon,double elev)219 void bkml_write::write_location(std::ofstream &ofs, const std::string& name, const std::string& description, double lat, double lon, double elev)
220 {
221   ofs << "<Placemark>\n"
222       << "  <name>" << name << "</name>\n"
223       << "  <description>" << description << "</description>\n"
224       << "  <styleUrl>#m_ylw-pushpin</styleUrl>\n"
225       << "  <Point>\n"
226       << "    <coordinates>" << std::setprecision(12) << lon << ", " << std::setprecision(12) << lat << ", " << elev << "</coordinates>\n"
227       << "  </Point>\n"
228       << "</Placemark>\n" << std::endl;
229 }
230 
write_location(std::ofstream & ofs,vgl_point_2d<double> const & loc,std::string const & name,std::string const & description,double const & scale,unsigned char const & r,unsigned char const & g,unsigned char const & b)231 void bkml_write::write_location(std::ofstream& ofs, vgl_point_2d<double> const& loc,
232                                 std::string const& name,
233                                 std::string const& description,
234                                 double const& scale,
235                                 unsigned char const& r,
236                                 unsigned char const& g,
237                                 unsigned char const& b)
238 {
239   std::string color = rgb_color_to_hex_color(255, (int)r, (int)g, (int)b);
240   ofs << "<Placemark>\n"
241       << "  <name>" << name << "</name>\n";
242   if (description.compare("") != 0)
243     ofs << "  <description>" << description << "</description>\n";
244   ofs << "  <Style>\n"
245       << "    <LabelStyle> <scale>" << scale << "</scale> </LabelStyle>\n"
246       << "    <IconStyle> <scale>" << scale << "</scale> <color>" << color << "</color>\n"
247       << "      <Icon><href>http://maps.google.com/mapfiles/kml/paddle/pink-blank.png</href></Icon>"
248       << "    </IconStyle>\n"
249       << "  </Style>\n"
250       << "  <Point>\n"
251       << "    <coordinates>" << std::setprecision(12) << loc.x() << ',' << std::setprecision(12) << loc.y() << ",0</coordinates>\n";
252   ofs << "  </Point>\n"
253       << "</Placemark>\n";
254 }
255 
write_location(std::ofstream & ofs,double lat,double lon,double elev,std::string const & name,std::string const & description,double const & scale,unsigned char const & r,unsigned char const & g,unsigned char const & b)256 void bkml_write::write_location(std::ofstream& ofs, double lat, double lon, double elev,
257                                 std::string const& name, std::string const& description, double const& scale,
258                                 unsigned char const& r, unsigned char const& g, unsigned char const& b)
259 {
260   std::string color = rgb_color_to_hex_color(255, (int)r, (int)g, (int)b);
261   ofs << "<Placemark>\n"
262       << "  <name>" << name << "</name>\n";
263   if (description.compare("") != 0)
264     ofs << "  <description>" << description << "</description>\n";
265   ofs << "  <Style>\n"
266       << "    <LabelStyle> <scale>" << scale << "</scale> </LabelStyle>\n"
267       << "    <IconStyle> <scale>" << scale << "</scale> <color>" << color << "</color>\n"
268       << "      <Icon><href>http://maps.google.com/mapfiles/kml/shapes/shaded_dot.png</href></Icon>"
269       << "    </IconStyle>\n"
270       << "  </Style>\n"
271       << "  <Point>\n"
272       << "    <coordinates>" << std::setprecision(12) << lon << ','
273                              << std::setprecision(12) << lat << ','
274                              << std::setprecision(12) << elev
275       << "</coordinates>\n";
276   ofs << "  </Point>\n"
277       << "</Placemark>\n";
278 }
279 
write_location_as_box(std::ofstream & ofs,double lon,double lat,double,std::string const & name,std::string const & description,double const & size,unsigned char const & r,unsigned char const & g,unsigned char const & b)280 void bkml_write::write_location_as_box(std::ofstream& ofs, double lon, double lat, double  /*elev*/,
281                                        std::string const& name,
282                                        std::string const& description,
283                                        double const& size,
284                                        unsigned char const& r, unsigned char const& g, unsigned char const& b)
285 {
286   vnl_double_2 ul, ll, lr, ur;
287   ll[0] = lat; ll[1] = lon;
288   ul[0] = lat+size; ul[1] = lon;
289   lr[0] = lat; lr[1] = lon+size;
290   ur[0] = lat+size; ur[1] = lon+size;
291   bkml_write::write_box(ofs, name, description, ul, ur, ll, lr, r, g, b);
292 }
293 
write_photo_overlay(std::ofstream & ofs,const std::string & name,double lon,double lat,double alt,double head,double tilt,double roll,double t_fov,double r_fov,double value)294 void bkml_write::write_photo_overlay(std::ofstream& ofs, const std::string& name,
295                                      double lon, double lat, double alt,
296                                      double head, double tilt, double roll,
297                                      double t_fov, double r_fov, double value)
298 {
299   if (value != 0.0) {
300     ofs << "<value>" << value << "</value>\n";
301   }
302   ofs << "<PhotoOverlay>\n"
303       << "  <name>" << name << "</name>\n"
304       << "  <Camera>\n"
305       << "    <longitude>" << std::setprecision(12) << lon << "</longitude>\n"
306       << "    <latitude>"  << std::setprecision(12) << lat << "</latitude>\n"
307       << "    <altitude>"  << alt << "</altitude>\n"
308       << "    <heading>"   << head << "</heading>\n"
309       << "    <tilt>"      << tilt << "</tilt>\n"
310       << "    <roll>"      << roll << "</roll>\n"
311       << "    <altitudeMode>relativeToGround</altitudeMode>\n"
312       << "  </Camera>\n"
313       << "  <ViewVolume>\n"
314       << "    <leftFov>"   << -1*r_fov << "</leftFov>\n"
315       << "    <rightFov>"  << r_fov    << "</rightFov>\n"
316       << "    <bottomFov>" << -1*t_fov << "</bottomFov>\n"
317       << "    <topFov>"    << t_fov    << "</topFov>\n"
318       << "    <near></near>\n"
319       << "  </ViewVolume>\n"
320       << "</PhotoOverlay>\n" << std::endl;
321 }
322 
write_kml_style(std::ofstream & ofs,const std::string & style_name,double const & scale,double const & line_width,double const & alpha,unsigned char const & r,unsigned char const & g,unsigned char const & b)323 void bkml_write::write_kml_style(std::ofstream& ofs,
324                                  const std::string& style_name,
325                                  double const& scale,
326                                  double const& line_width,
327                                  double const& alpha,
328                                  unsigned char const& r,
329                                  unsigned char const& g,
330                                  unsigned char const& b)
331 {
332   // obtain line color
333   std::string line_color = rgb_color_to_hex_color(1, (int)b, (int)r, (int)g);
334 
335   // obtain polygon color
336   int alpha_int = (int)(alpha*255);
337   std::string poly_color = rgb_color_to_hex_color(alpha_int, (int)b, (int)g, (int)r);
338 
339   ofs << "<Style id=\"" << style_name << "\">\n"
340       << "  <LableStyle> <scale>" << scale << "</scale> </LabelStyle>\n"
341       << "  <LineStyle> <color>" << line_color << "</color> <width>" << line_width << "</width> </LineStyle>\n"
342       << "  <PolyStyle>\n"
343       << "    <color>" << poly_color << "</color>\n"
344       << "    <fill>1</fill>\n"
345       << "    <outline>0</outline>\n"
346       << "  </PolyStyle>\n"
347       << "</Style>\n";
348 }
349 
write_polygon(std::ofstream & ofs,std::vector<std::pair<vgl_polygon<double>,vgl_polygon<double>>> const & polygon,std::string const & name,std::string const & description,double const & scale,double const & line_width,double const & alpha,unsigned char const & r,unsigned char const & g,unsigned char const & b)350 void bkml_write::write_polygon(std::ofstream& ofs,
351                                std::vector<std::pair<vgl_polygon<double>, vgl_polygon<double> > > const& polygon,
352                                std::string const& name, std::string const& description,
353                                double const& scale, double const& line_width,
354                                double const& alpha, unsigned char const& r, unsigned char const& g, unsigned char const& b)
355 {
356   // obtain line color
357   std::string line_color = rgb_color_to_hex_color(255, (int)r, (int)g, (int)b);
358   // obtain polygon color
359   int alpha_int = (int)(alpha*255);
360   std::string poly_color = rgb_color_to_hex_color(alpha_int, (int)r, (int)g, (int)b);
361 
362   for (const auto & i : polygon) {
363     vgl_polygon<double> outer = i.first;
364     vgl_polygon<double> inner = i.second;
365     if (outer[0].empty())
366       continue;
367     ofs << "<Placemark>\n"
368         << "  <name>" << name << "</name>\n";
369     if (description.compare("") != 0)
370       ofs << "  <description>" << description << "</description>\n";
371     ofs << "  <Style>\n"
372         << "    <LabelStyle> <scale>" << scale << "</scale> </LabelStyle>\n"
373         << "    <LineStyle> <color>" << line_color << "</color> <width>" << line_width << "</width> </LineStyle>\n"
374         << "    <PolyStyle>\n"
375         << "      <color>" << poly_color << "</color>\n"
376         << "      <fill>0</fill>\n"
377         << "      <outline>1</outline>\n"
378         << "    </PolyStyle>\n"
379         << "  </Style>\n";
380     ofs << "  <Polygon>\n"
381         << "    <tessellate>1</tessellate>\n"
382         << "    <outerBoundaryIs>\n"
383         << "      <LinearRing>\n"
384         << "        <coordinates>";
385     for (auto & vit : outer[0])
386       ofs << std::setprecision(12) << vit.x() << ',' << std::setprecision(12) << vit.y() << ",0 ";
387     ofs << std::setprecision(12) << outer[0][0].x() << ',' << std::setprecision(12) << outer[0][0].y() << ",0";
388     ofs << "</coordinates>\n"
389         << "      </LinearRing>\n"
390         << "    </outerBoundaryIs>\n";
391     // write out the inner boundary
392     for (unsigned in_idx = 0; in_idx < inner.num_sheets(); in_idx++) {
393       ofs << "    <innerBoundaryIs>\n"
394           << "      <LinearRing>\n"
395           << "        <coordinates>";
396       for (auto & vit : inner[in_idx])
397         ofs << std::setprecision(12) << vit.x() << ',' << std::setprecision(12) << vit.y() << ",0 ";
398       ofs << std::setprecision(12) << inner[in_idx][0].x() << ',' << std::setprecision(12) << inner[in_idx][0].y() << ",0";
399       ofs << "</coordinates>\n"
400           << "      </LinearRing>\n"
401           << "    </innerBoundaryIs>\n";
402     }
403     ofs << "  </Polygon>\n"
404         << "</Placemark>\n";
405   }
406   return;
407 }
408