root/OpenSceneGraph/trunk/examples/osggpx/osggpx.cpp @ 13318

Revision 12292, 21.1 kB (checked in by robert, 3 years ago)

Ran svn propset -R svn:eol-style native . on the OpenSceneGraph

  • Property svn:eol-style set to native
Line 
1/* OpenSceneGraph example, osggpx.
2*
3*  Permission is hereby granted, free of charge, to any person obtaining a copy
4*  of this software and associated documentation files (the "Software"), to deal
5*  in the Software without restriction, including without limitation the rights
6*  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
7*  copies of the Software, and to permit persons to whom the Software is
8*  furnished to do so, subject to the following conditions:
9*
10*  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
11*  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
12*  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
13*  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
14*  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
15*  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
16*  THE SOFTWARE.
17*/
18
19#include <osg/Node>
20#include <osg/Geometry>
21#include <osg/Notify>
22#include <osg/MatrixTransform>
23#include <osg/Texture2D>
24#include <osg/DrawPixels>
25#include <osg/PolygonOffset>
26#include <osg/Geode>
27#include <osg/CoordinateSystemNode>
28
29#include <osgDB/Registry>
30#include <osgDB/ReadFile>
31#include <osgDB/FileUtils>
32#include <osgDB/FileNameUtils>
33#include <osgDB/XmlParser>
34
35#include <osgText/Text>
36
37#include <osgGA/TerrainManipulator>
38#include <osgViewer/Viewer>
39
40class TrackSegment : public osg::Object
41{
42public:
43    TrackSegment() {}
44
45    TrackSegment(const TrackSegment& ts, const osg::CopyOp=osg::CopyOp::SHALLOW_COPY) {}
46
47    META_Object(osg, TrackSegment)
48
49    struct TrackPoint
50    {
51        TrackPoint():
52            latitude(0.0),
53            longitude(0.0),
54            elevation(0.0),
55            time(0.0) {}
56
57        double latitude;
58        double longitude;
59        double elevation;
60        double time;
61    };
62
63    typedef std::vector< TrackPoint > TrackPoints;
64
65    void addTrackPoint(const TrackPoint& trackPoint) { _trackPoints.push_back(trackPoint); }
66
67    TrackPoints& getTrackPoints() { return _trackPoints; }
68    const TrackPoints& getTrackPoints() const { return _trackPoints; }
69
70protected:
71    virtual ~TrackSegment() {}
72
73    TrackPoints _trackPoints;
74};
75
76class Track : public osg::Object
77{
78public:
79    Track() {}
80
81    Track(const Track& track, const osg::CopyOp=osg::CopyOp::SHALLOW_COPY) {}
82
83    META_Object(osg, Track)
84
85    typedef std::vector< osg::ref_ptr<TrackSegment> > TrackSegments;
86
87    void addTrackSegment(TrackSegment* trackSegment) { _trackSegments.push_back(trackSegment); }
88
89    TrackSegments& getTrackSegments() { return _trackSegments; }
90    const TrackSegments& getTrackSegments() const { return _trackSegments; }
91
92protected:
93
94    virtual ~Track() {}
95
96    TrackSegments _trackSegments;
97};
98
99double convertTime(const std::string& timestr)
100{
101    osg::notify(osg::NOTICE)<<"       time = "<<timestr<<std::endl;
102    return 0;
103}
104
105Track* readTrack(const std::string& filename)
106{
107    std::string foundFilename = osgDB::findDataFile(filename);
108    if (foundFilename.empty()) return 0;
109
110    std::string ext = osgDB::getFileExtension(foundFilename);
111    if (ext!="gpx") return 0;
112
113    osgDB::XmlNode::Input input;
114    input.open(foundFilename);
115    input.readAllDataIntoBuffer();
116
117    osg::ref_ptr<osgDB::XmlNode> doc = new osgDB::XmlNode;
118    doc->read(input);
119
120    osgDB::XmlNode* root = 0;
121    for(osgDB::XmlNode::Children::iterator itr = doc->children.begin();
122        itr != doc->children.end() && !root;
123        ++itr)
124    {
125        if ((*itr)->name=="gpx") root = itr->get();
126    }
127
128    if (!root) return 0;
129
130    std::string latitude("lat");
131    std::string longitude("lon");
132
133    for(osgDB::XmlNode::Children::iterator itr = root->children.begin();
134        itr != root->children.end();
135        ++itr)
136    {
137        if ((*itr)->name=="rte")
138        {
139            osg::ref_ptr<Track> track = new Track;
140            track->setName(filename);
141
142            osg::ref_ptr<TrackSegment> trackSegment = new TrackSegment;
143            for(osgDB::XmlNode::Children::iterator sitr = (*itr)->children.begin();
144                sitr != (*itr)->children.end();
145                ++sitr)
146            {
147                if ((*sitr)->name=="rtept" )
148                {
149                    osgDB::XmlNode* trkpt = sitr->get();
150                    TrackSegment::TrackPoint point;
151                    bool valid = false;
152                    if (trkpt->properties.count(latitude)!=0)
153                    {
154                        valid = true;
155                        point.latitude = osg::asciiToDouble(trkpt->properties[latitude].c_str());
156                    }
157                    if (trkpt->properties.count(longitude)!=0)
158                    {
159                        valid = true;
160                        point.longitude = osg::asciiToDouble(trkpt->properties[longitude].c_str());
161                    }
162
163                    for(osgDB::XmlNode::Children::iterator pitr = trkpt->children.begin();
164                        pitr != trkpt->children.end();
165                        ++pitr)
166                    {
167                        if ((*pitr)->name=="ele") point.elevation = osg::asciiToDouble((*pitr)->contents.c_str());
168                        else if ((*pitr)->name=="time") point.time = convertTime((*pitr)->contents);
169                    }
170
171                    if (valid)
172                    {
173                        osg::notify(osg::NOTICE)<<"  point.latitude="<<point.latitude<<", longitude="<<point.longitude<<", elev="<<point.elevation<<", time="<<point.time<<std::endl;
174                        trackSegment->addTrackPoint(point);
175                    }
176                }
177
178            }
179
180            if (!trackSegment->getTrackPoints().empty())
181            {
182                track->addTrackSegment(trackSegment.get());
183            }
184
185            return track.release();
186        }
187        else  if ((*itr)->name=="trk")
188        {
189            osg::ref_ptr<Track> track = new Track;
190            track->setName(filename);
191
192            for(osgDB::XmlNode::Children::iterator citr = (*itr)->children.begin();
193                citr != (*itr)->children.end();
194                ++citr)
195            {
196                if ((*citr)->name=="trkseg")
197                {
198                    osg::ref_ptr<TrackSegment> trackSegment = new TrackSegment;
199                    for(osgDB::XmlNode::Children::iterator sitr = (*citr)->children.begin();
200                        sitr != (*citr)->children.end();
201                        ++sitr)
202                    {
203                        if ((*sitr)->name=="trkpt" || (*sitr)->name=="rtept" )
204                        {
205                            osgDB::XmlNode* trkpt = sitr->get();
206                            TrackSegment::TrackPoint point;
207                            bool valid = false;
208                            if (trkpt->properties.count(latitude)!=0)
209                            {
210                                valid = true;
211                                point.latitude = osg::asciiToDouble(trkpt->properties[latitude].c_str());
212                            }
213                            if (trkpt->properties.count(longitude)!=0)
214                            {
215                                valid = true;
216                                point.longitude = osg::asciiToDouble(trkpt->properties[longitude].c_str());
217                            }
218
219                            for(osgDB::XmlNode::Children::iterator pitr = trkpt->children.begin();
220                                pitr != trkpt->children.end();
221                                ++pitr)
222                            {
223                                if ((*pitr)->name=="ele") point.elevation = osg::asciiToDouble((*pitr)->contents.c_str());
224                                else if ((*pitr)->name=="time") point.time = convertTime((*pitr)->contents);
225                            }
226
227                            if (valid)
228                            {
229                                // osg::notify(osg::NOTICE)<<"  point.latitude="<<point.latitude<<", longitude="<<point.longitude<<", elev="<<point.elevation<<", time="<<point.time<<std::endl;
230                                trackSegment->addTrackPoint(point);
231                            }
232                        }
233                    }
234                    if (!trackSegment->getTrackPoints().empty())
235                    {
236                        track->addTrackSegment(trackSegment.get());
237                    }
238                }
239            }
240            return track.release();
241        }
242    }
243
244    return 0;
245}
246
247TrackSegment* computeSmoothedTrackSegment(TrackSegment* ts)
248{
249    if (!ts) return 0;
250
251    const TrackSegment::TrackPoints& orig_points = ts->getTrackPoints();
252
253    if (orig_points.size()>2)
254    {
255        // only do smoothing if we have more than two points.
256        osg::ref_ptr<TrackSegment> new_ts = new TrackSegment;
257
258        TrackSegment::TrackPoints& new_points = new_ts->getTrackPoints();
259        new_points.resize(orig_points.size());
260
261        new_points[0] = orig_points[0];
262        new_points[orig_points.size()-1] = orig_points[orig_points.size()-1];
263
264        for(unsigned int i=1; i<orig_points.size()-1; ++i)
265        {
266            new_points[i].latitude = (orig_points[i-1].latitude+orig_points[i].latitude+orig_points[i+1].latitude)/3.0;
267            new_points[i].longitude = (orig_points[i-1].longitude+orig_points[i].longitude+orig_points[i+1].longitude)/3.0;
268            new_points[i].elevation = (orig_points[i-1].elevation+orig_points[i].elevation+orig_points[i+1].elevation)/3.0;
269            new_points[i].time = (orig_points[i-1].time+orig_points[i].time+orig_points[i+1].time)/3.0;
270        }
271        return new_ts.release();
272    }
273    else
274    {
275        // we have two or less points and can't do smoothing, so will just return original TrackSegment
276        return ts;
277    }
278
279}
280
281TrackSegment* computeAveragedSpeedTrackSegment(TrackSegment* ts)
282{
283    if (!ts) return 0;
284
285    osg::ref_ptr<osg::EllipsoidModel> em = new osg::EllipsoidModel;
286    const TrackSegment::TrackPoints& orig_points = ts->getTrackPoints();
287
288    if (orig_points.size()>2)
289    {
290        // only do smoothing if we have more than two points.
291        osg::ref_ptr<TrackSegment> new_ts = new TrackSegment;
292
293
294        // compute overall distance
295        double total_distance = 0;
296        for(unsigned int i=1; i<orig_points.size()-1; ++i)
297        {
298            osg::Vec3d point_a, point_b;
299            em->convertLatLongHeightToXYZ(osg::DegreesToRadians(orig_points[i].latitude), osg::DegreesToRadians(orig_points[i].longitude), orig_points[i].elevation,
300                                          point_a.x(), point_a.y(), point_a.z());
301            em->convertLatLongHeightToXYZ(osg::DegreesToRadians(orig_points[i+1].latitude), osg::DegreesToRadians(orig_points[i+1].longitude), orig_points[i+1].elevation,
302                                          point_b.x(), point_b.y(), point_b.z());
303            total_distance += (point_b-point_a).length();
304        }
305
306        double total_time = orig_points[orig_points.size()-1].time - orig_points[0].time;
307        double average_speed = total_distance/total_time;
308
309        OSG_NOTICE<<"total_time = "<<total_time<<std::endl;
310        OSG_NOTICE<<"total_distance = "<<total_distance<<std::endl;
311        OSG_NOTICE<<"average_speed = "<<average_speed<<std::endl;
312
313        TrackSegment::TrackPoints& new_points = new_ts->getTrackPoints();
314        new_points.resize(orig_points.size());
315        new_points[0] = orig_points[0];
316
317        double accumulated_distance = 0.0;
318        for(unsigned int i=0; i<orig_points.size()-1; ++i)
319        {
320            osg::Vec3d point_a, point_b;
321            em->convertLatLongHeightToXYZ(osg::DegreesToRadians(orig_points[i].latitude), osg::DegreesToRadians(orig_points[i].longitude), orig_points[i].elevation,
322                                          point_a.x(), point_a.y(), point_a.z());
323            em->convertLatLongHeightToXYZ(osg::DegreesToRadians(orig_points[i+1].latitude), osg::DegreesToRadians(orig_points[i+1].longitude), orig_points[i+1].elevation,
324                                          point_b.x(), point_b.y(), point_b.z());
325
326            accumulated_distance += (point_b-point_a).length();
327
328            new_points[i+1] = orig_points[i+1];
329            new_points[i+1].time = accumulated_distance / average_speed;
330        }
331        return new_ts.release();
332    }
333    else
334    {
335        // we have two or less points and can't do smoothing, so will just return original TrackSegment
336        return ts;
337    }
338
339}
340
341Track* computeAveragedSpeedTrack(Track* track)
342{
343    osg::ref_ptr<Track> new_track = new Track;
344
345    for(Track::TrackSegments::iterator itr = track->getTrackSegments().begin();
346        itr != track->getTrackSegments().end();
347        ++itr)
348    {
349        new_track->addTrackSegment(computeAveragedSpeedTrackSegment(itr->get()));
350    }
351
352    return new_track.release();
353}
354
355
356Track* computeSmoothedTrack(Track* track)
357{
358    osg::ref_ptr<Track> new_track = new Track;
359
360    for(Track::TrackSegments::iterator itr = track->getTrackSegments().begin();
361        itr != track->getTrackSegments().end();
362        ++itr)
363    {
364        new_track->addTrackSegment(computeSmoothedTrackSegment(itr->get()));
365    }
366
367    return new_track.release();
368}
369
370osg::Node* createTrackModel(Track* track, const osg::Vec4& colour)
371{
372    osg::ref_ptr<osg::EllipsoidModel> em = new osg::EllipsoidModel;
373
374    osg::ref_ptr<osg::Geode> geode = new osg::Geode;
375
376    for(Track::TrackSegments::iterator itr = track->getTrackSegments().begin();
377        itr != track->getTrackSegments().end();
378        ++itr)
379    {
380        const TrackSegment::TrackPoints& points = (*itr)->getTrackPoints();
381        if (points.size()<2) continue;
382
383        osg::ref_ptr<osg::Geometry> geometry = new osg::Geometry;
384        osg::ref_ptr<osg::Vec3Array> vertices = new osg::Vec3Array;
385        geometry->setVertexArray(vertices.get());
386        vertices->resize(points.size());
387        for(unsigned int i=0; i<points.size(); ++i)
388        {
389            osg::Vec3d point;
390            em->convertLatLongHeightToXYZ(osg::DegreesToRadians(points[i].latitude), osg::DegreesToRadians(points[i].longitude), points[i].elevation,
391                                          point.x(), point.y(), point.z());
392
393           (*vertices)[i] = point;
394        }
395
396        osg::ref_ptr<osg::Vec4Array> colours = new osg::Vec4Array;
397        colours->push_back(colour);
398        geometry->setColorArray(colours.get());
399        geometry->setColorBinding(osg::Geometry::BIND_OVERALL);
400
401        geometry->addPrimitiveSet(new osg::DrawArrays(GL_LINE_STRIP, 0, points.size()));
402
403        geode->addDrawable(geometry.get());
404    }
405
406    geode->getOrCreateStateSet()->setMode(GL_LIGHTING, osg::StateAttribute::OFF);
407
408    return geode.release();
409}
410
411int main(int argv, char **argc)
412{
413    osg::ArgumentParser arguments(&argv, argc);
414
415    typedef std::list< osg::ref_ptr<Track> > Tracks;
416    Tracks tracks;
417
418    bool average = false;
419    while (arguments.read("-a") || arguments.read("--average")) average = true;
420
421    bool smooth = false;
422    while (arguments.read("-s") || arguments.read("--smooth")) smooth = true;
423
424    std::string outputFilename;
425    while (arguments.read("-o",outputFilename)) {}
426
427    std::string trackFilename;
428    while (arguments.read("-t",trackFilename))
429    {
430        osg::ref_ptr<Track> track = readTrack(trackFilename);
431        if (track.valid()) tracks.push_back(track.get());
432    }
433
434    osg::ref_ptr<osg::EllipsoidModel> em = new osg::EllipsoidModel;
435
436    osg::ref_ptr<osg::Group> group = new osg::Group;
437
438    osg::ref_ptr<osg::Node> loadedModel = osgDB::readNodeFiles(arguments);
439    if (loadedModel.valid()) group->addChild(loadedModel.get());
440
441    for(Tracks::iterator itr = tracks.begin();
442        itr != tracks.end();
443        ++itr)
444    {
445        Track* track = itr->get();
446
447        group->addChild(createTrackModel(track, osg::Vec4(1.0,1.0,1.0,1.0)));
448
449        // smooth the track
450        if (average)
451        {
452            for(Track::TrackSegments::iterator itr = track->getTrackSegments().begin();
453                itr != track->getTrackSegments().end();
454                ++itr)
455            {
456                *itr = computeAveragedSpeedTrackSegment(itr->get());
457            }
458        }
459
460        // smooth the track
461        if (smooth)
462        {
463            for(Track::TrackSegments::iterator itr = track->getTrackSegments().begin();
464                itr != track->getTrackSegments().end();
465                ++itr)
466            {
467                *itr = computeSmoothedTrackSegment(itr->get());
468            }
469
470            for(Track::TrackSegments::iterator itr = track->getTrackSegments().begin();
471                itr != track->getTrackSegments().end();
472                ++itr)
473            {
474                *itr = computeSmoothedTrackSegment(itr->get());
475            }
476
477            for(Track::TrackSegments::iterator itr = track->getTrackSegments().begin();
478                itr != track->getTrackSegments().end();
479                ++itr)
480            {
481                *itr = computeSmoothedTrackSegment(itr->get());
482            }
483
484            for(Track::TrackSegments::iterator itr = track->getTrackSegments().begin();
485                itr != track->getTrackSegments().end();
486                ++itr)
487            {
488                *itr = computeSmoothedTrackSegment(itr->get());
489            }
490        }
491
492        double totalDistance = 0.0;
493        double totalAscent = 0.0;
494        double totalDescent = 0.0;
495
496        osg::notify(osg::NOTICE)<<"Track read "<<track->getName()<<std::endl;
497        for(Track::TrackSegments::iterator itr = track->getTrackSegments().begin();
498            itr != track->getTrackSegments().end();
499            ++itr)
500        {
501            TrackSegment* ts = itr->get();
502
503            const TrackSegment::TrackPoints& points = ts->getTrackPoints();
504            if (points.size()>1)
505            {
506                TrackSegment::TrackPoints::iterator pitr = ts->getTrackPoints().begin();
507                osg::Vec3d previousPos;
508                double previousElevation = pitr->elevation;
509                em->convertLatLongHeightToXYZ(osg::DegreesToRadians(pitr->latitude), osg::DegreesToRadians(pitr->longitude), 0.0,
510                                              previousPos.x(), previousPos.y(), previousPos.z());
511                ++pitr;
512
513                for(;
514                    pitr != ts->getTrackPoints().end();
515                    ++pitr)
516                {
517                    osg::Vec3d newPos;
518                    double newElevation = pitr->elevation;
519                    em->convertLatLongHeightToXYZ(osg::DegreesToRadians(pitr->latitude), osg::DegreesToRadians(pitr->longitude), 0.0,
520                                                newPos.x(), newPos.y(), newPos.z());
521
522                    double distance = (newPos-previousPos).length();
523
524                    totalDistance += distance;
525                    if (newElevation>previousElevation) totalAscent += (newElevation-previousElevation);
526                    else totalDescent += (previousElevation-newElevation);
527
528                    osg::notify(osg::NOTICE)<<"     distance="<<distance<<", "<<newElevation-previousElevation<<std::endl;
529
530                    previousPos = newPos;
531                    previousElevation = newElevation;
532                }
533            }
534        }
535
536        double metersToFeet = 1 / 0.3048;
537        double metersToMiles = 1.0 / 1609.344;
538
539        osg::notify(osg::NOTICE)<<"totalDistance = "<<totalDistance<<"m, "<<totalDistance*metersToMiles<<" miles"<<std::endl;
540        osg::notify(osg::NOTICE)<<"totalAscent = "<<totalAscent<<"m, "<<totalAscent*metersToFeet<<"ft"<<std::endl;
541        osg::notify(osg::NOTICE)<<"totalDescent = "<<totalDescent<<"m, "<<totalDescent*metersToFeet<<"ft"<<std::endl;
542
543    }
544
545    if (!outputFilename.empty())
546    {
547        std::ofstream fout(outputFilename.c_str());
548
549        fout<<"<?xml version=\"1.0\" encoding=\"utf-8\"?><gpx version=\"1.0\" creator=\"osggpx\" xmlns:xsi=\"http://www.w3.org/2001/XMLSchema-instance\" xmlns=\"http://www.topografix.com/GPX/1/0\" xsi:schemaLocation=\"http://www.topografix.com/GPX/1/0 http://www.topografix.com/GPX/1/0/gpx.xsd\">"<<std::endl;
550
551        for(Tracks::iterator itr = tracks.begin();
552            itr != tracks.end();
553            ++itr)
554        {
555            Track* track = itr->get();
556
557            fout<<"<trk>"<<std::endl;
558            fout<<"<desc>The track description</desc>"<<std::endl;
559            for(Track::TrackSegments::iterator itr = track->getTrackSegments().begin();
560                itr != track->getTrackSegments().end();
561                ++itr)
562            {
563                TrackSegment* ts = itr->get();
564                fout<<"<trkseg>"<<std::endl;
565
566                for(TrackSegment::TrackPoints::iterator pitr = ts->getTrackPoints().begin();
567                    pitr != ts->getTrackPoints().end();
568                    ++pitr)
569                {
570                    fout<<"<trkpt lat=\""<<pitr->latitude<<"\" lon=\""<<pitr->longitude<<"\">"<<std::endl;
571                    fout<<"<ele>"<<pitr->elevation<<"</ele>"<<std::endl;
572                    fout<<"<time>"<<pitr->time<<"</time>"<<std::endl;
573                    fout<<"</trkpt>"<<std::endl;
574                }
575
576                fout<<"</trkseg>"<<std::endl;
577
578            }
579            fout<<"</trk>"<<std::endl;
580        }
581        fout<<"</gpx>"<<std::endl;
582    }
583
584    osgViewer::Viewer viewer(arguments);
585    viewer.setCameraManipulator(new osgGA::TerrainManipulator);
586    viewer.setSceneData(group.get());
587    return viewer.run();
588
589}
Note: See TracBrowser for help on using the browser.