Branch data Line data Source code
1 : : /*
2 : : ===============================================================================
3 : :
4 : : FILE: field_gpstime.hpp
5 : :
6 : : CONTENTS:
7 : :
8 : :
9 : : PROGRAMMERS:
10 : :
11 : : martin.isenburg@rapidlasso.com - http://rapidlasso.com
12 : : uday.karan@gmail.com - Hobu, Inc.
13 : :
14 : : COPYRIGHT:
15 : :
16 : : (c) 2007-2014, martin isenburg, rapidlasso - tools to catch reality
17 : : (c) 2014, Uday Verma, Hobu, Inc.
18 : :
19 : : This is free software; you can redistribute and/or modify it under the
20 : : terms of the GNU Lesser General Licence as published by the Free Software
21 : : Foundation. See the COPYING file for more information.
22 : :
23 : : This software is distributed WITHOUT ANY WARRANTY and without even the
24 : : implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
25 : :
26 : : CHANGE HISTORY:
27 : :
28 : : ===============================================================================
29 : : */
30 : :
31 : : #ifndef __las_hpp__
32 : : #error Cannot directly include this file, this is a part of las.hpp
33 : : #endif
34 : :
35 : : #define LASZIP_GPSTIME_MULTI 500
36 : : #define LASZIP_GPSTIME_MULTI_MINUS -10
37 : : #define LASZIP_GPSTIME_MULTI_UNCHANGED (LASZIP_GPSTIME_MULTI - LASZIP_GPSTIME_MULTI_MINUS + 1)
38 : : #define LASZIP_GPSTIME_MULTI_CODE_FULL (LASZIP_GPSTIME_MULTI - LASZIP_GPSTIME_MULTI_MINUS + 2)
39 : :
40 : : #define LASZIP_GPSTIME_MULTI_TOTAL (LASZIP_GPSTIME_MULTI - LASZIP_GPSTIME_MULTI_MINUS + 6)
41 : :
42 : : namespace laszip {
43 : : namespace formats {
44 : : // Teach packers how to pack and unpack gps time
45 : : //
46 : : template<>
47 : : struct packers<las::gpstime> {
48 : 0 : inline static las::gpstime unpack(const char *in) {
49 : 0 : uint64_t lower = packers<unsigned int>::unpack(in),
50 : 0 : upper = packers<unsigned int>::unpack(in + 4);
51 : :
52 : 0 : return las::gpstime((upper << 32) | lower);
53 : : }
54 : :
55 : 0 : inline static void pack(const las::gpstime& t, char *buffer) {
56 : 0 : packers<unsigned int>::pack(t.value & 0xFFFFFFFF, buffer);
57 : 0 : packers<unsigned int>::pack(t.value >> 32, buffer + 4);
58 : 0 : }
59 : : };
60 : :
61 : : // Figure how to compress and decompress GPS time fields
62 : : //
63 : : template<>
64 : 0 : struct field<las::gpstime> {
65 : : typedef las::gpstime type;
66 : :
67 : 0 : field() : compressor_inited_(false), decompressor_inited_(false) {}
68 : :
69 : : template<
70 : : typename TEncoder
71 : : >
72 : : inline const char *compressWith(TEncoder& enc, const char *buf)
73 : : {
74 : : las::gpstime this_val = packers<las::gpstime>::unpack(buf);
75 : :
76 : : if (!compressor_inited_) {
77 : : compressors_.init();
78 : : compressor_inited_ = true;
79 : : }
80 : :
81 : : if (!common_.have_last_) {
82 : : // don't have the first data yet, just push it to our have last stuff and move on
83 : : common_.have_last_ = true;
84 : : common_.last_gpstime[0] = this_val;
85 : :
86 : : // write this out to the encoder as it is
87 : : enc.getOutStream().putBytes((const unsigned char*)buf,
88 : : sizeof(las::gpstime));
89 : : buf += sizeof(las::gpstime);
90 : :
91 : : // we are done here
92 : : return buf;
93 : : }
94 : :
95 : : if (common_.last_gpstime_diff[common_.last] == 0) { // if last integer different was 0
96 : : if (this_val.value == common_.last_gpstime[common_.last].value) {
97 : : enc.encodeSymbol(common_.m_gpstime_0diff, 0);
98 : : }
99 : : else {
100 : : // calculate the difference between the two doubles as an integer
101 : : //
102 : : int64_t curr_gpstime_diff_64 = this_val.value - common_.last_gpstime[common_.last].value;
103 : : int curr_gpstime_diff = static_cast<int>(curr_gpstime_diff_64);
104 : :
105 : : if (curr_gpstime_diff_64 == static_cast<int64_t>(curr_gpstime_diff)) {
106 : : // this difference is small enough to be represented with 32 bits
107 : : enc.encodeSymbol(common_.m_gpstime_0diff, 1);
108 : : compressors_.ic_gpstime.compress(enc, 0, curr_gpstime_diff, 0);
109 : : common_.last_gpstime_diff[common_.last] = curr_gpstime_diff;
110 : : common_.multi_extreme_counter[common_.last] = 0;
111 : : }
112 : : else { // the difference is huge
113 : : U32 i;
114 : :
115 : : // maybe the double belongs to another time sequence
116 : : //
117 : : for (i = 1; i < 4; i++) {
118 : : int64_t other_gpstime_diff_64 = this_val.value -
119 : : common_.last_gpstime[(common_.last+i)&3].value;
120 : : int other_gpstime_diff = static_cast<int>(other_gpstime_diff_64);
121 : :
122 : : if (other_gpstime_diff_64 == static_cast<int64_t>(other_gpstime_diff)) {
123 : : enc.encodeSymbol(common_.m_gpstime_0diff, i+2); // it belongs to another sequence
124 : : common_.last = (common_.last+i)&3;
125 : : return compressWith(enc, buf);
126 : : }
127 : : }
128 : :
129 : : // no other sequence found. start new sequence.
130 : : enc.encodeSymbol(common_.m_gpstime_0diff, 2);
131 : : compressors_.ic_gpstime.compress(enc,
132 : : static_cast<int>(common_.last_gpstime[common_.last].value >> 32),
133 : : static_cast<int>(this_val.value >> 32), 8);
134 : :
135 : : enc.writeInt(static_cast<unsigned int>(this_val.value));
136 : :
137 : : common_.next = (common_.next+1)&3;
138 : : common_.last = common_.next;
139 : : common_.last_gpstime_diff[common_.last] = 0;
140 : : common_.multi_extreme_counter[common_.last] = 0;
141 : : }
142 : : common_.last_gpstime[common_.last] = this_val;
143 : : }
144 : : }
145 : : else { // the last integer difference was *not* zero
146 : : if (this_val.value == common_.last_gpstime[common_.last].value) {
147 : : // if the doubles have not changed use a special symbol
148 : : enc.encodeSymbol(common_.m_gpstime_multi, LASZIP_GPSTIME_MULTI_UNCHANGED);
149 : : }
150 : : else
151 : : {
152 : : // calculate the difference between the two doubles as an integer
153 : : int64_t curr_gpstime_diff_64 = this_val.value -
154 : : common_.last_gpstime[common_.last].value;
155 : : int curr_gpstime_diff = static_cast<int>(curr_gpstime_diff_64);
156 : :
157 : : // if the current gpstime difference can be represented with 32 bits
158 : : if (curr_gpstime_diff_64 == static_cast<int64_t>(curr_gpstime_diff)) {
159 : : // compute multiplier between current and last integer difference
160 : : float multi_f = (float)curr_gpstime_diff /
161 : : (float)(common_.last_gpstime_diff[common_.last]);
162 : : int multi = I32_QUANTIZE(multi_f);
163 : :
164 : : // compress the residual curr_gpstime_diff in dependance on the multiplier
165 : : if (multi == 1) {
166 : : // this is the case we assume we get most often for regular spaced pulses
167 : : enc.encodeSymbol(common_.m_gpstime_multi, 1);
168 : : compressors_.ic_gpstime.compress(enc,
169 : : common_.last_gpstime_diff[common_.last], curr_gpstime_diff, 1);
170 : : common_.multi_extreme_counter[common_.last] = 0;
171 : : }
172 : : else if (multi > 0) {
173 : : if (multi < LASZIP_GPSTIME_MULTI) {
174 : : // positive multipliers up to LASZIP_GPSTIME_MULTI are compressed directly
175 : : enc.encodeSymbol(common_.m_gpstime_multi, multi);
176 : : if (multi < 10)
177 : : compressors_.ic_gpstime.compress(enc,
178 : : multi*common_.last_gpstime_diff[common_.last],
179 : : curr_gpstime_diff, 2);
180 : : else
181 : : compressors_.ic_gpstime.compress(enc,
182 : : multi*common_.last_gpstime_diff[common_.last],
183 : : curr_gpstime_diff, 3);
184 : : }
185 : : else {
186 : : enc.encodeSymbol(common_.m_gpstime_multi, LASZIP_GPSTIME_MULTI);
187 : : compressors_.ic_gpstime.compress(enc,
188 : : LASZIP_GPSTIME_MULTI*common_.last_gpstime_diff[common_.last],
189 : : curr_gpstime_diff, 4);
190 : : common_.multi_extreme_counter[common_.last]++;
191 : :
192 : : if (common_.multi_extreme_counter[common_.last] > 3) {
193 : : common_.last_gpstime_diff[common_.last] = curr_gpstime_diff;
194 : : common_.multi_extreme_counter[common_.last] = 0;
195 : : }
196 : : }
197 : : }
198 : : else if (multi < 0) {
199 : : if (multi > LASZIP_GPSTIME_MULTI_MINUS) {
200 : : // negative multipliers larger than LASZIP_GPSTIME_MULTI_MINUS are compressed directly
201 : : enc.encodeSymbol(common_.m_gpstime_multi, LASZIP_GPSTIME_MULTI - multi);
202 : : compressors_.ic_gpstime.compress(enc,
203 : : multi*common_.last_gpstime_diff[common_.last],
204 : : curr_gpstime_diff, 5);
205 : : }
206 : : else {
207 : : enc.encodeSymbol(common_.m_gpstime_multi, LASZIP_GPSTIME_MULTI - LASZIP_GPSTIME_MULTI_MINUS);
208 : : compressors_.ic_gpstime.compress(enc,
209 : : LASZIP_GPSTIME_MULTI_MINUS*common_.last_gpstime_diff[common_.last],
210 : : curr_gpstime_diff, 6);
211 : :
212 : : common_.multi_extreme_counter[common_.last]++;
213 : : if (common_.multi_extreme_counter[common_.last] > 3)
214 : : {
215 : : common_.last_gpstime_diff[common_.last] = curr_gpstime_diff;
216 : : common_.multi_extreme_counter[common_.last] = 0;
217 : : }
218 : : }
219 : : }
220 : : else {
221 : : enc.encodeSymbol(common_.m_gpstime_multi, 0);
222 : : compressors_.ic_gpstime.compress(enc, 0, curr_gpstime_diff, 7);
223 : : common_.multi_extreme_counter[common_.last]++;
224 : : if (common_.multi_extreme_counter[common_.last] > 3)
225 : : {
226 : : common_.last_gpstime_diff[common_.last] = curr_gpstime_diff;
227 : : common_.multi_extreme_counter[common_.last] = 0;
228 : : }
229 : : }
230 : : }
231 : : else { // the difference is huge
232 : : int i;
233 : : // maybe the double belongs to another time sequence
234 : : for (i = 1; i < 4; i++)
235 : : {
236 : : int64_t other_gpstime_diff_64 = this_val.value - common_.last_gpstime[(common_.last+i)&3].value;
237 : : int other_gpstime_diff = static_cast<int>(other_gpstime_diff_64);
238 : :
239 : : if (other_gpstime_diff_64 == static_cast<int64_t>(other_gpstime_diff)) {
240 : : // it belongs to this sequence
241 : : enc.encodeSymbol(common_.m_gpstime_multi, LASZIP_GPSTIME_MULTI_CODE_FULL+i);
242 : : common_.last = (common_.last+i)&3;
243 : : return compressWith(enc, buf);
244 : : }
245 : : }
246 : :
247 : : // no other sequence found. start new sequence.
248 : : enc.encodeSymbol(common_.m_gpstime_multi, LASZIP_GPSTIME_MULTI_CODE_FULL);
249 : : compressors_.ic_gpstime.compress(
250 : : enc,
251 : : static_cast<int>(common_.last_gpstime[common_.last].value >> 32),
252 : : static_cast<int>(this_val.value >> 32), 8);
253 : : enc.writeInt(static_cast<unsigned int>(this_val.value));
254 : : common_.next = (common_.next+1)&3;
255 : : common_.last = common_.next;
256 : : common_.last_gpstime_diff[common_.last] = 0;
257 : : common_.multi_extreme_counter[common_.last] = 0;
258 : : }
259 : :
260 : : common_.last_gpstime[common_.last] = this_val;
261 : : }
262 : : }
263 : : return buf + sizeof(las::gpstime);
264 : : }
265 : :
266 : : template<
267 : : typename TDecoder
268 : : >
269 : 0 : inline char *decompressWith(TDecoder& dec, char *buf) {
270 : 0 : if (!decompressor_inited_) {
271 : 0 : decompressors_.init();
272 : 0 : decompressor_inited_ = true;
273 : 0 : }
274 : :
275 : 0 : if (!common_.have_last_) {
276 : : // don't have the first data yet, read the whole point out of the stream
277 : 0 : common_.have_last_ = true;
278 : :
279 : 0 : dec.getInStream().getBytes((unsigned char*)buf,
280 : : sizeof(las::gpstime));
281 : : // decode this value
282 : 0 : common_.last_gpstime[0] = packers<las::gpstime>::unpack(buf);
283 : :
284 : : // we are done here
285 : 0 : return buf + sizeof(las::gpstime);
286 : : }
287 : :
288 : : int multi;
289 : 0 : if (common_.last_gpstime_diff[common_.last] == 0) { // if the last integer difference was zero
290 : 0 : multi = dec.decodeSymbol(common_.m_gpstime_0diff);
291 : :
292 : 0 : if (multi == 1) { // the difference can be represented with 32 bits
293 : 0 : common_.last_gpstime_diff[common_.last] = decompressors_.ic_gpstime.decompress(dec, 0, 0);
294 : 0 : common_.last_gpstime[common_.last].value += common_.last_gpstime_diff[common_.last];
295 : 0 : common_.multi_extreme_counter[common_.last] = 0;
296 : 0 : }
297 : 0 : else if (multi == 2) { // the difference is huge
298 : 0 : common_.next = (common_.next+1)&3;
299 : 0 : common_.last_gpstime[common_.next].value = decompressors_.ic_gpstime.decompress(
300 : 0 : dec,
301 : 0 : (common_.last_gpstime[common_.last].value >> 32), 8);
302 : 0 : common_.last_gpstime[common_.next].value = common_.last_gpstime[common_.next].value << 32;
303 : 0 : common_.last_gpstime[common_.next].value |= dec.readInt();
304 : 0 : common_.last = common_.next;
305 : 0 : common_.last_gpstime_diff[common_.last] = 0;
306 : 0 : common_.multi_extreme_counter[common_.last] = 0;
307 : 0 : }
308 : 0 : else if (multi > 2) { // we switch to another sequence
309 : 0 : common_.last = (common_.last+multi-2)&3;
310 : :
311 : 0 : decompressWith(dec, buf);
312 : 0 : }
313 : 0 : }
314 : : else {
315 : 0 : multi = dec.decodeSymbol(common_.m_gpstime_multi);
316 : 0 : if (multi == 1) {
317 : 0 : common_.last_gpstime[common_.last].value += decompressors_.ic_gpstime.decompress(
318 : 0 : dec,
319 : 0 : common_.last_gpstime_diff[common_.last], 1);
320 : 0 : common_.multi_extreme_counter[common_.last] = 0;
321 : 0 : }
322 : 0 : else if (multi < LASZIP_GPSTIME_MULTI_UNCHANGED) {
323 : : int gpstime_diff;
324 : 0 : if (multi == 0) {
325 : 0 : gpstime_diff = decompressors_.ic_gpstime.decompress(dec, 0, 7);
326 : 0 : common_.multi_extreme_counter[common_.last]++;
327 : 0 : if (common_.multi_extreme_counter[common_.last] > 3) { common_.last_gpstime_diff[common_.last] = gpstime_diff;
328 : 0 : common_.multi_extreme_counter[common_.last] = 0;
329 : 0 : }
330 : 0 : }
331 : 0 : else if (multi < LASZIP_GPSTIME_MULTI) {
332 : 0 : if (multi < 10)
333 : 0 : gpstime_diff = decompressors_.ic_gpstime.decompress(dec,
334 : 0 : multi*common_.last_gpstime_diff[common_.last], 2);
335 : : else
336 : 0 : gpstime_diff = decompressors_.ic_gpstime.decompress(dec,
337 : 0 : multi*common_.last_gpstime_diff[common_.last], 3);
338 : 0 : }
339 : 0 : else if (multi == LASZIP_GPSTIME_MULTI) {
340 : 0 : gpstime_diff = decompressors_.ic_gpstime.decompress(
341 : 0 : dec,
342 : 0 : LASZIP_GPSTIME_MULTI*common_.last_gpstime_diff[common_.last], 4);
343 : 0 : common_.multi_extreme_counter[common_.last]++;
344 : 0 : if (common_.multi_extreme_counter[common_.last] > 3) {
345 : 0 : common_.last_gpstime_diff[common_.last] = gpstime_diff;
346 : 0 : common_.multi_extreme_counter[common_.last] = 0;
347 : 0 : }
348 : 0 : }
349 : : else {
350 : 0 : multi = LASZIP_GPSTIME_MULTI - multi;
351 : 0 : if (multi > LASZIP_GPSTIME_MULTI_MINUS) {
352 : 0 : gpstime_diff = decompressors_.ic_gpstime.decompress(
353 : 0 : dec,
354 : 0 : multi*common_.last_gpstime_diff[common_.last], 5);
355 : 0 : }
356 : : else
357 : : {
358 : 0 : gpstime_diff = decompressors_.ic_gpstime.decompress(
359 : 0 : dec,
360 : 0 : LASZIP_GPSTIME_MULTI_MINUS*common_.last_gpstime_diff[common_.last], 6);
361 : 0 : common_.multi_extreme_counter[common_.last]++;
362 : 0 : if (common_.multi_extreme_counter[common_.last] > 3) {
363 : 0 : common_.last_gpstime_diff[common_.last] = gpstime_diff;
364 : 0 : common_.multi_extreme_counter[common_.last] = 0;
365 : 0 : }
366 : : }
367 : : }
368 : 0 : common_.last_gpstime[common_.last].value += gpstime_diff;
369 : 0 : }
370 : 0 : else if (multi == LASZIP_GPSTIME_MULTI_CODE_FULL) {
371 : 0 : common_.next = (common_.next+1)&3;
372 : 0 : common_.last_gpstime[common_.next].value = decompressors_.ic_gpstime.decompress(
373 : 0 : dec, static_cast<int>(common_.last_gpstime[common_.last].value >> 32), 8);
374 : 0 : common_.last_gpstime[common_.next].value = common_.last_gpstime[common_.next].value << 32;
375 : 0 : common_.last_gpstime[common_.next].value |= dec.readInt();
376 : 0 : common_.last = common_.next;
377 : 0 : common_.last_gpstime_diff[common_.last] = 0;
378 : 0 : common_.multi_extreme_counter[common_.last] = 0;
379 : 0 : }
380 : 0 : else if (multi >= LASZIP_GPSTIME_MULTI_CODE_FULL) {
381 : 0 : common_.last = (common_.last+multi-LASZIP_GPSTIME_MULTI_CODE_FULL)&3;
382 : :
383 : 0 : decompressWith(dec, buf);
384 : 0 : }
385 : : }
386 : 0 : packers<las::gpstime>::pack(common_.last_gpstime[common_.last],
387 : 0 : buf);
388 : 0 : return buf + sizeof(las::gpstime);
389 : 0 : }
390 : :
391 : : // All the things we need to compress a point, group them into structs
392 : : // so we don't have too many names flying around
393 : :
394 : : // Common parts for both a compressor and decompressor go here
395 : : struct __common {
396 : : bool have_last_;
397 : : models::arithmetic m_gpstime_multi, m_gpstime_0diff;
398 : : unsigned int last, next;
399 : : std::array<las::gpstime, 4> last_gpstime;
400 : : std::array<int, 4> last_gpstime_diff;
401 : : std::array<int, 4> multi_extreme_counter;
402 : :
403 : 0 : __common() :
404 : 0 : have_last_(false),
405 : 0 : m_gpstime_multi(LASZIP_GPSTIME_MULTI_TOTAL),
406 : 0 : m_gpstime_0diff(6),
407 : 0 : last(0), next(0) {
408 : :
409 : 0 : last_gpstime.fill(las::gpstime());
410 : 0 : last_gpstime_diff.fill(0);
411 : 0 : multi_extreme_counter.fill(0);
412 : 0 : }
413 : :
414 : :
415 : 0 : ~__common() {
416 : 0 : }
417 : : } common_;
418 : :
419 : : // These compressors are specific to a compressor usage, so we keep them separate here
420 : 0 : struct __compressors {
421 : : compressors::integer ic_gpstime;
422 : :
423 : 0 : __compressors() :
424 : 0 : ic_gpstime(32, 9) {}
425 : :
426 : : void init() {
427 : : ic_gpstime.init();
428 : : }
429 : : } compressors_;
430 : :
431 : 0 : struct __decompressors {
432 : : decompressors::integer ic_gpstime;
433 : :
434 : 0 : __decompressors() :
435 : 0 : ic_gpstime(32, 9) {}
436 : :
437 : 0 : void init() {
438 : 0 : ic_gpstime.init();
439 : 0 : }
440 : : } decompressors_;
441 : :
442 : : bool compressor_inited_;
443 : : bool decompressor_inited_;
444 : : };
445 : : }
446 : : }
|
