Root/monav/gpsgrid/cell.h

1/*
2Copyright 2010 Christian Vetter veaac.fdirct@gmail.com
3
4This file is part of MoNav.
5
6MoNav is free software: you can redistribute it and/or modify
7it under the terms of the GNU General Public License as published by
8the Free Software Foundation, either version 3 of the License, or
9(at your option) any later version.
10
11MoNav is distributed in the hope that it will be useful,
12but WITHOUT ANY WARRANTY; without even the implied warranty of
13MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14GNU General Public License for more details.
15
16You should have received a copy of the GNU General Public License
17along with MoNav. If not, see <http://www.gnu.org/licenses/>.
18*/
19
20#ifndef CELL_H
21#define CELL_H
22
23#include "utils/config.h"
24#include "utils/coordinates.h"
25#include "utils/bithelpers.h"
26#include "utils/edgeconnector.h"
27#include <vector>
28#include <algorithm>
29
30namespace gg
31{
32
33    class Cell {
34
35    public:
36
37        struct Edge {
38            unsigned pathID;
39            unsigned short pathLength;
40            unsigned short edgeID;
41            NodeID source;
42            NodeID target;
43            bool bidirectional;
44            bool operator==( const Edge& right ) const {
45                if ( source != right.source )
46                    return false;
47                if ( target != right.target )
48                    return false;
49                if ( bidirectional != right.bidirectional )
50                    return false;
51                if ( edgeID != right.edgeID )
52                    return false;
53                if ( pathLength != right.pathLength )
54                    return false;
55                return true;
56            }
57        };
58
59        std::vector< Edge > edges;
60        std::vector< UnsignedCoordinate > coordinates;
61
62        bool operator==( const Cell& right ) const
63        {
64            if ( edges.size() != right.edges.size() )
65                return false;
66            for ( int i = 0; i < ( int ) edges.size(); i++ ) {
67                if ( !( edges[i] == right.edges[i] ) )
68                    return false;
69                for ( unsigned path = 0; path < edges[i].pathLength; path++ ) {
70                    if ( coordinates[path + edges[i].pathID].x != right.coordinates[path + right.edges[i].pathID].x )
71                        return false;
72                    if ( coordinates[path + edges[i].pathID].y != right.coordinates[path + right.edges[i].pathID].y )
73                        return false;
74                }
75            }
76
77            return true;
78        }
79
80        size_t write( unsigned char* buffer, UnsignedCoordinate min, UnsignedCoordinate max )
81        {
82            unsigned char* const oldBuffer = buffer;
83
84            NodeID minID = std::numeric_limits< NodeID >::max();
85            NodeID maxID = 0;
86            unsigned short maxEdgeID = 0;
87            unsigned short maxPathLength = 0;
88
89            {
90                // build edge connector data structures
91                std::vector< EdgeConnector< unsigned >::Edge > connectorEdges;
92                std::vector< unsigned > resultSegments;
93                std::vector< unsigned > resultSegmentDescriptions;
94                std::vector< bool > resultReversed;
95
96                for ( unsigned i = 0; i < ( unsigned ) edges.size(); i++ ) {
97                    EdgeConnector< unsigned >::Edge newEdge;
98                    newEdge.source = edges[i].source;
99                    newEdge.target = edges[i].target;
100                    newEdge.reverseable = edges[i].bidirectional;
101                    connectorEdges.push_back( newEdge );
102                }
103
104                EdgeConnector< unsigned >::run( &resultSegments, &resultSegmentDescriptions, &resultReversed, connectorEdges );
105
106                for ( unsigned i = 0; i < ( unsigned ) edges.size(); i++ ) {
107                    if ( resultReversed[i] ) {
108                        std::swap( edges[i].source, edges[i].target );
109                        std::reverse( coordinates.begin() + edges[i].pathID, coordinates.begin() + edges[i].pathID + edges[i].pathLength );
110                    }
111                }
112
113                std::vector< Edge > reorderedEdges;
114                for ( unsigned i = 0; i < ( unsigned ) resultSegmentDescriptions.size(); i++ )
115                    reorderedEdges.push_back( edges[resultSegmentDescriptions[i]] );
116
117                edges.swap( reorderedEdges );
118            }
119
120            std::vector< NodeID > nodes;
121            for ( std::vector< Edge >::iterator i = edges.begin(), e = edges.end(); i != e; ++i ) {
122                minID = std::min( minID, i->source );
123                minID = std::min( minID, i->target );
124                maxID = std::max( maxID, i->source );
125                maxID = std::max( maxID, i->target );
126                maxID = std::max( maxID, i->target );
127                maxEdgeID = std::max( maxEdgeID, i->edgeID );
128                assert( i->pathLength > 1 );
129                maxPathLength = std::max( maxPathLength, ( unsigned short ) ( i->pathLength - 2 ) );
130
131                nodes.push_back( i->source );
132                nodes.push_back( i->target );
133            }
134
135            std::sort( nodes.begin(), nodes.end() );
136            nodes.resize( std::unique( nodes.begin(), nodes.end() ) - nodes.begin() );
137
138            const char xBits = bits_needed( max.x - min.x );
139            const char yBits = bits_needed( max.y - min.y );
140            const char idBits = bits_needed( maxID - minID );
141            const char edgeIDBits = bits_needed( maxEdgeID );
142            const char pathLengthBits = bits_needed( maxPathLength );
143
144            int offset = 0;
145            write_unaligned_unsigned( &buffer, edges.size(), 32, &offset );
146            write_unaligned_unsigned( &buffer, minID, 32, &offset );
147            write_unaligned_unsigned( &buffer, idBits, 6, &offset );
148            write_unaligned_unsigned( &buffer, edgeIDBits, 6, &offset );
149            write_unaligned_unsigned( &buffer, pathLengthBits, 6, &offset );
150
151            NodeID lastTarget = std::numeric_limits< NodeID >::max();
152            std::vector< UnsignedCoordinate > wayBuffer;
153            for ( std::vector< Edge >::const_iterator i = edges.begin(), e = edges.end(); i != e; i++ ) {
154                bool reuse = lastTarget == i->source;
155
156                write_unaligned_unsigned( &buffer, reuse ? 1 : 0, 1, &offset );
157                if ( !reuse )
158                    write_unaligned_unsigned( &buffer, i->source - minID, idBits, &offset );
159                write_unaligned_unsigned( &buffer, i->target - minID, idBits, &offset );
160                write_unaligned_unsigned( &buffer, i->edgeID, edgeIDBits, &offset );
161                write_unaligned_unsigned( &buffer, i->bidirectional ? 1 : 0, 1, &offset );
162                write_unaligned_unsigned( &buffer, i->pathLength - 2, pathLengthBits, &offset );
163
164                assert( i->pathLength >= 2 );
165                for ( int pathID = 1; pathID < i->pathLength - 1; pathID++ )
166                    wayBuffer.push_back( coordinates[pathID + i->pathID] );
167
168                lastTarget = i->target;
169            }
170
171            std::vector< UnsignedCoordinate > nodeCoordinates( nodes.size() );
172            for ( std::vector< Edge >::iterator i = edges.begin(), e = edges.end(); i != e; ++i ) {
173                unsigned sourcePos = lower_bound( nodes.begin(), nodes.end(), i->source ) - nodes.begin();
174                unsigned targetPos = lower_bound( nodes.begin(), nodes.end(), i->target ) - nodes.begin();
175                nodeCoordinates[sourcePos] = coordinates[i->pathID];
176                nodeCoordinates[targetPos] = coordinates[i->pathID + i->pathLength - 1];
177            }
178
179            for ( std::vector< UnsignedCoordinate >::const_iterator i = nodeCoordinates.begin(), iend = nodeCoordinates.end(); i != iend; i++ ) {
180                writeCoordinate( &buffer, &offset, i->x, min.x, max.x, xBits);
181                writeCoordinate( &buffer, &offset, i->y, min.y, max.y, yBits);
182            }
183
184            for ( std::vector< UnsignedCoordinate >::const_iterator i = wayBuffer.begin(), iend = wayBuffer.end(); i != iend; i++ ) {
185                writeCoordinate( &buffer, &offset, i->x, min.x, max.x, xBits);
186                writeCoordinate( &buffer, &offset, i->y, min.y, max.y, yBits);
187            }
188
189            buffer += ( offset + 7 ) / 8;
190
191            return buffer - oldBuffer;
192        }
193
194        size_t read( const unsigned char* buffer, UnsignedCoordinate min, UnsignedCoordinate max ) {
195            const unsigned char* oldBuffer = buffer;
196
197            int offset = 0;
198
199            const char xBits = bits_needed( max.x - min.x );
200            const char yBits = bits_needed( max.y - min.y );
201
202            unsigned numEdges = read_unaligned_unsigned( &buffer, 32, &offset );
203            unsigned minID = read_unaligned_unsigned( &buffer, 32, &offset );
204            unsigned idBits = read_unaligned_unsigned( &buffer, 6, &offset );
205            unsigned edgeIDBits = read_unaligned_unsigned( &buffer, 6, &offset );
206            unsigned pathLengthBits = read_unaligned_unsigned( &buffer, 6, &offset );
207
208            std::vector< NodeID > nodes;
209            NodeID lastTarget = std::numeric_limits< NodeID >::max();
210            for ( unsigned i = 0; i < numEdges; i++ ) {
211                Edge edge;
212                bool reuse = read_unaligned_unsigned( &buffer, 1, &offset ) == 1;
213                if ( reuse )
214                    edge.source = lastTarget;
215                else
216                    edge.source = read_unaligned_unsigned( &buffer, idBits, &offset ) + minID;
217                edge.target = read_unaligned_unsigned( &buffer, idBits, &offset ) + minID;
218                edge.edgeID = read_unaligned_unsigned( &buffer, edgeIDBits, &offset );
219                edge.bidirectional = read_unaligned_unsigned( &buffer, 1, &offset ) == 1;
220                edge.pathLength = read_unaligned_unsigned( &buffer, pathLengthBits, &offset );
221
222                edges.push_back( edge );
223
224                nodes.push_back( edge.source );
225                nodes.push_back( edge.target );
226
227                lastTarget = edge.target;
228            }
229            assert( edges.size() != 0 );
230
231            std::sort( nodes.begin(), nodes.end() );
232            nodes.resize( std::unique( nodes.begin(), nodes.end() ) - nodes.begin() );
233
234            std::vector< UnsignedCoordinate > nodeCoordinates( nodes.size() );
235            for ( std::vector< UnsignedCoordinate >::iterator i = nodeCoordinates.begin(), iend = nodeCoordinates.end(); i != iend; i++ ) {
236                readCoordinate( &buffer, &offset, &i->x, min.x, max.x, xBits);
237                readCoordinate( &buffer, &offset, &i->y, min.y, max.y, yBits);
238            }
239
240            for ( std::vector< Edge >::iterator i = edges.begin(), iend = edges.end(); i != iend; i++ ) {
241                unsigned sourcePos = lower_bound( nodes.begin(), nodes.end(), i->source ) - nodes.begin();
242                unsigned targetPos = lower_bound( nodes.begin(), nodes.end(), i->target ) - nodes.begin();
243                if ( coordinates.size() == 0 || coordinates.back().x != nodeCoordinates[sourcePos].x || coordinates.back().y != nodeCoordinates[sourcePos].y )
244                    coordinates.push_back( nodeCoordinates[sourcePos] );
245                i->pathID = coordinates.size() - 1;
246                for ( int path = 0; path < i->pathLength; path++ ) {
247                    UnsignedCoordinate coordinate;
248                    readCoordinate( &buffer, &offset, &coordinate.x, min.x, max.x, xBits);
249                    readCoordinate( &buffer, &offset, &coordinate.y, min.y, max.y, yBits);
250                    coordinates.push_back( coordinate );
251                }
252                coordinates.push_back( nodeCoordinates[targetPos] );
253
254                i->pathLength += 2;
255            }
256
257            buffer += ( offset + 7 ) / 8;
258
259            return buffer - oldBuffer;
260        }
261
262    protected:
263
264        void writeCoordinate( unsigned char** buffer, int* offset, unsigned value, unsigned min, unsigned max, char bits )
265        {
266            bool inside = !( value < min || value > max );
267            write_unaligned_unsigned( buffer, inside ? 1 : 0, 1, offset );
268            if ( inside )
269                write_unaligned_unsigned( buffer, value - min, bits, offset );
270            else
271                write_unaligned_unsigned( buffer, value, 32, offset );
272        }
273
274        void readCoordinate( const unsigned char** buffer, int* offset, unsigned* value, unsigned min, unsigned /*max*/, char bits )
275        {
276            bool inside = read_unaligned_unsigned( buffer, 1, offset ) == 1;
277            if ( inside )
278                *value = read_unaligned_unsigned( buffer, bits, offset ) + min;
279            else
280                *value = read_unaligned_unsigned( buffer, 32, offset );
281        }
282
283    };
284}
285
286#endif // CELL_H
287

Archive Download this file

Branches:
master



interactive