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

Revision 13574, 21.0 kB (checked in by robert, 15 hours ago)

From Jason Beverage, "It looks like the Callback header got accidentally removed from the CMakeLists.txt in the submission yesterday for the geometry instancing example."

  • 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(), osg::Array::BIND_OVERALL);
399
400        geometry->addPrimitiveSet(new osg::DrawArrays(GL_LINE_STRIP, 0, points.size()));
401
402        geode->addDrawable(geometry.get());
403    }
404
405    geode->getOrCreateStateSet()->setMode(GL_LIGHTING, osg::StateAttribute::OFF);
406
407    return geode.release();
408}
409
410int main(int argv, char **argc)
411{
412    osg::ArgumentParser arguments(&argv, argc);
413
414    typedef std::list< osg::ref_ptr<Track> > Tracks;
415    Tracks tracks;
416
417    bool average = false;
418    while (arguments.read("-a") || arguments.read("--average")) average = true;
419
420    bool smooth = false;
421    while (arguments.read("-s") || arguments.read("--smooth")) smooth = true;
422
423    std::string outputFilename;
424    while (arguments.read("-o",outputFilename)) {}
425
426    std::string trackFilename;
427    while (arguments.read("-t",trackFilename))
428    {
429        osg::ref_ptr<Track> track = readTrack(trackFilename);
430        if (track.valid()) tracks.push_back(track.get());
431    }
432
433    osg::ref_ptr<osg::EllipsoidModel> em = new osg::EllipsoidModel;
434
435    osg::ref_ptr<osg::Group> group = new osg::Group;
436
437    osg::ref_ptr<osg::Node> loadedModel = osgDB::readNodeFiles(arguments);
438    if (loadedModel.valid()) group->addChild(loadedModel.get());
439
440    for(Tracks::iterator itr = tracks.begin();
441        itr != tracks.end();
442        ++itr)
443    {
444        Track* track = itr->get();
445
446        group->addChild(createTrackModel(track, osg::Vec4(1.0,1.0,1.0,1.0)));
447
448        // smooth the track
449        if (average)
450        {
451            for(Track::TrackSegments::iterator itr = track->getTrackSegments().begin();
452                itr != track->getTrackSegments().end();
453                ++itr)
454            {
455                *itr = computeAveragedSpeedTrackSegment(itr->get());
456            }
457        }
458
459        // smooth the track
460        if (smooth)
461        {
462            for(Track::TrackSegments::iterator itr = track->getTrackSegments().begin();
463                itr != track->getTrackSegments().end();
464                ++itr)
465            {
466                *itr = computeSmoothedTrackSegment(itr->get());
467            }
468
469            for(Track::TrackSegments::iterator itr = track->getTrackSegments().begin();
470                itr != track->getTrackSegments().end();
471                ++itr)
472            {
473                *itr = computeSmoothedTrackSegment(itr->get());
474            }
475
476            for(Track::TrackSegments::iterator itr = track->getTrackSegments().begin();
477                itr != track->getTrackSegments().end();
478                ++itr)
479            {
480                *itr = computeSmoothedTrackSegment(itr->get());
481            }
482
483            for(Track::TrackSegments::iterator itr = track->getTrackSegments().begin();
484                itr != track->getTrackSegments().end();
485                ++itr)
486            {
487                *itr = computeSmoothedTrackSegment(itr->get());
488            }
489        }
490
491        double totalDistance = 0.0;
492        double totalAscent = 0.0;
493        double totalDescent = 0.0;
494
495        osg::notify(osg::NOTICE)<<"Track read "<<track->getName()<<std::endl;
496        for(Track::TrackSegments::iterator itr = track->getTrackSegments().begin();
497            itr != track->getTrackSegments().end();
498            ++itr)
499        {
500            TrackSegment* ts = itr->get();
501
502            const TrackSegment::TrackPoints& points = ts->getTrackPoints();
503            if (points.size()>1)
504            {
505                TrackSegment::TrackPoints::iterator pitr = ts->getTrackPoints().begin();
506                osg::Vec3d previousPos;
507                double previousElevation = pitr->elevation;
508                em->convertLatLongHeightToXYZ(osg::DegreesToRadians(pitr->latitude), osg::DegreesToRadians(pitr->longitude), 0.0,
509                                              previousPos.x(), previousPos.y(), previousPos.z());
510                ++pitr;
511
512                for(;
513                    pitr != ts->getTrackPoints().end();
514                    ++pitr)
515                {
516                    osg::Vec3d newPos;
517                    double newElevation = pitr->elevation;
518                    em->convertLatLongHeightToXYZ(osg::DegreesToRadians(pitr->latitude), osg::DegreesToRadians(pitr->longitude), 0.0,
519                                                newPos.x(), newPos.y(), newPos.z());
520
521                    double distance = (newPos-previousPos).length();
522
523                    totalDistance += distance;
524                    if (newElevation>previousElevation) totalAscent += (newElevation-previousElevation);
525                    else totalDescent += (previousElevation-newElevation);
526
527                    osg::notify(osg::NOTICE)<<"     distance="<<distance<<", "<<newElevation-previousElevation<<std::endl;
528
529                    previousPos = newPos;
530                    previousElevation = newElevation;
531                }
532            }
533        }
534
535        double metersToFeet = 1 / 0.3048;
536        double metersToMiles = 1.0 / 1609.344;
537
538        osg::notify(osg::NOTICE)<<"totalDistance = "<<totalDistance<<"m, "<<totalDistance*metersToMiles<<" miles"<<std::endl;
539        osg::notify(osg::NOTICE)<<"totalAscent = "<<totalAscent<<"m, "<<totalAscent*metersToFeet<<"ft"<<std::endl;
540        osg::notify(osg::NOTICE)<<"totalDescent = "<<totalDescent<<"m, "<<totalDescent*metersToFeet<<"ft"<<std::endl;
541
542    }
543
544    if (!outputFilename.empty())
545    {
546        std::ofstream fout(outputFilename.c_str());
547
548        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;
549
550        for(Tracks::iterator itr = tracks.begin();
551            itr != tracks.end();
552            ++itr)
553        {
554            Track* track = itr->get();
555
556            fout<<"<trk>"<<std::endl;
557            fout<<"<desc>The track description</desc>"<<std::endl;
558            for(Track::TrackSegments::iterator itr = track->getTrackSegments().begin();
559                itr != track->getTrackSegments().end();
560                ++itr)
561            {
562                TrackSegment* ts = itr->get();
563                fout<<"<trkseg>"<<std::endl;
564
565                for(TrackSegment::TrackPoints::iterator pitr = ts->getTrackPoints().begin();
566                    pitr != ts->getTrackPoints().end();
567                    ++pitr)
568                {
569                    fout<<"<trkpt lat=\""<<pitr->latitude<<"\" lon=\""<<pitr->longitude<<"\">"<<std::endl;
570                    fout<<"<ele>"<<pitr->elevation<<"</ele>"<<std::endl;
571                    fout<<"<time>"<<pitr->time<<"</time>"<<std::endl;
572                    fout<<"</trkpt>"<<std::endl;
573                }
574
575                fout<<"</trkseg>"<<std::endl;
576
577            }
578            fout<<"</trk>"<<std::endl;
579        }
580        fout<<"</gpx>"<<std::endl;
581    }
582
583    osgViewer::Viewer viewer(arguments);
584    viewer.setCameraManipulator(new osgGA::TerrainManipulator);
585    viewer.setSceneData(group.get());
586    return viewer.run();
587
588}
Note: See TracBrowser for help on using the browser.