DTrackSDK  v2.9.0
DTrackParser.cpp
1 /* DTrackSDK in C++: DTrackParser.cpp
2  *
3  * Functions to process DTrack UDP packets (ASCII protocol).
4  *
5  * Copyright (c) 2013-2024 Advanced Realtime Tracking GmbH & Co. KG
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions are met:
9  *
10  * 1. Redistributions of source code must retain the above copyright
11  * notice, this list of conditions and the following disclaimer.
12  * 2. Redistributions in binary form must reproduce the above copyright
13  * notice, this list of conditions and the following disclaimer in the
14  * documentation and/or other materials provided with the distribution.
15  * 3. Neither the name of copyright holder nor the names of its contributors
16  * may be used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
20  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
21  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
23  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
24  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
25  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
26  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
27  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
28  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
29  *
30  * Purpose:
31  * - DTrack network protocol according to:
32  * 'DTrack2 User Manual, Technical Appendix' or 'DTRACK3 Programmer's Guide'
33  */
34 
35 #include "DTrackParser.hpp"
36 #include "DTrackParse.hpp"
37 
38 #include <cstring>
39 
40 #if ! defined( _MSC_VER )
41  #define strcpy_s( a, b, c ) strcpy( a, c ) // map 'strcpy_s' if not Visual Studio
42  #define strcat_s( a, b, c ) strcat( a, c ) // map 'strcat_s' if not Visual Studio
43 #endif
44 
45 using namespace DTrackSDK_Parse;
46 
47 
49 static void reduced_to_full_cov( double* cov_full, const double* cov_reduced, int dim )
50 {
51  for (int r=0; r<dim; ++r) {
52  int k = r*(r-1)/2;
53  cov_full[ r*(dim+1) ] = cov_reduced[ r*dim - k ];
54  for (int c=r+1; c<dim; ++c) {
55  cov_full[ r*dim + c ] = cov_full[ c*dim + r ] = cov_reduced[ r*(dim-1) - k + c ];
56  }
57  }
58 }
59 
60 
61 /*
62  * Constructor.
63  */
65 {
66  // reset actual DTrack data:
67  act_framecounter = 0;
68  act_timestamp = -1;
69  act_timestamp_sec = 0;
70  act_timestamp_usec = 0;
71  act_latency_usec = 0;
72 
73  act_num_body = act_num_flystick = act_num_meatool = act_num_mearef = act_num_hand = act_num_human = 0;
74  act_num_inertial = 0;
75  act_num_marker = 0;
76 
77  act_is_status_available = false;
78 }
79 
80 
81 /*
82  * Destructor.
83  */
85 {
86  //
87 }
88 
89 
90 /*
91  * Set default values at start of a new frame.
92  */
94 {
95  act_framecounter = 0;
96  act_timestamp = -1; // i.e. not available
97  act_timestamp_sec = 0;
98  act_timestamp_usec = 0;
99  act_latency_usec = 0;
100  act_is_status_available = false;
101 
102  loc_num_bodycal = loc_num_handcal = -1; // i.e. not available
103  loc_num_flystick1 = loc_num_meatool1 = 0;
104 }
105 
106 
107 /*
108  * Final adjustments after processing all data for a frame.
109  */
111 {
112  int j, n;
113 
114  // set number of calibrated standard bodies, if necessary:
115  if (loc_num_bodycal >= 0) { // '6dcal' information was available
116  n = loc_num_bodycal - loc_num_flystick1 - loc_num_meatool1;
117  if (n > act_num_body) { // adjust length of vector
118  act_body.resize(n);
119  for (j=act_num_body; j<n; j++) {
120  memset(&act_body[j], 0, sizeof(DTrack_Body_Type_d));
121  act_body[j].id = j;
122  act_body[j].quality = -1;
123  }
124  }
125  act_num_body = n;
126  }
127 
128  // set number of calibrated Fingertracking hands, if necessary:
129  if (loc_num_handcal >= 0) { // 'glcal' information was available
130  if (loc_num_handcal > act_num_hand) { // adjust length of vector
131  act_hand.resize(loc_num_handcal);
132  for (j=act_num_hand; j<loc_num_handcal; j++) {
133  memset(&act_hand[j], 0, sizeof(DTrack_Hand_Type_d));
134  act_hand[j].id = j;
135  act_hand[j].quality = -1;
136  }
137  }
138  act_num_hand = loc_num_handcal;
139  }
140 }
141 
142 
143 /*
144  * Parses a single line of data in one tracking data packet.
145  */
146 bool DTrackParser::parseLine(char **line)
147 {
148  if (!line)
149  return false;
150 
151  // line of frame counter:
152  if (!strncmp(*line, "fr ", 3)) {
153  *line += 3;
154  return parseLine_fr(line);
155  }
156 
157  // line of timestamp:
158  if ( strncmp( *line, "ts ", 3 ) == 0 )
159  {
160  *line += 3;
161  return parseLine_ts( line );
162  }
163 
164  // line of extended timestamp:
165  if ( strncmp( *line, "ts2 ", 4 ) == 0 )
166  {
167  *line += 4;
168  return parseLine_ts2( line );
169  }
170 
171  // line of additional information about number of calibrated bodies:
172  if ( strncmp( *line, "6dcal ", 6 ) == 0 )
173  {
174  *line += 6;
175  return parseLine_6dcal( line );
176  }
177 
178  // line of standard body data:
179  if (!strncmp(*line, "6d ", 3)) {
180  *line += 3;
181  return parseLine_6d(line);
182  }
183 
184  // line of 6d covariance data:
185  if (!strncmp(*line, "6dcov ", 6)) {
186  *line += 6;
187  return parseLine_6dcov(line);
188  }
189 
190  // line of Flystick data (older format):
191  if (!strncmp(*line, "6df ", 4)) {
192  *line += 4;
193  return parseLine_6df(line);
194  }
195 
196  // line of Flystick data (newer format):
197  if (!strncmp(*line, "6df2 ", 5)) {
198  *line += 5;
199  return parseLine_6df2(line);
200  }
201 
202  // line of measurement tool data (older format):
203  if (!strncmp(*line, "6dmt ", 5)) {
204  *line += 5;
205  return parseLine_6dmt(line);
206  }
207 
208  // line of measurement tool data (newer format):
209  if (!strncmp(*line, "6dmt2 ", 6)) {
210  *line += 6;
211  return parseLine_6dmt2(line);
212  }
213 
214  // line of measurement reference data:
215  if (!strncmp(*line, "6dmtr ", 6)) {
216  *line += 6;
217  return parseLine_6dmtr(line);
218  }
219 
220  // line of additional inofmation about number of calibrated Fingertracking hands:
221  if (!strncmp(*line, "glcal ", 6)) {
222  *line += 6;
223  return parseLine_glcal(line);
224  }
225 
226  // line of A.R.T. Fingertracking hand data:
227  if (!strncmp(*line, "gl ", 3)) {
228  *line += 3;
229  return parseLine_gl(line);
230  }
231 
232  // line of 6dj human model data:
233  if (!strncmp(*line, "6dj ", 4)) {
234  *line += 4;
235  return parseLine_6dj(line);
236  }
237 
238  // line of 6di inertial data:
239  if (!strncmp(*line, "6di ", 4)) {
240  *line += 4;
241  return parseLine_6di(line);
242  }
243 
244  // line of single marker data:
245  if (!strncmp(*line, "3d ", 3)) {
246  *line += 3;
247  return parseLine_3d(line);
248  }
249 
250  // line of system status data:
251  if ( strncmp( *line, "st ", 3 ) == 0 )
252  {
253  *line += 3;
254  return parseLine_st( line );
255  }
256 
257  return true; // ignore unknown line identifiers (could be valid in future DTracks)
258 }
259 
260 
261 /*
262  * Parses a single line of frame counter data in one tracking data packet.
263  */
264 bool DTrackParser::parseLine_fr(char **line)
265 {
266  *line = string_get_ui( *line, &act_framecounter );
267  if ( *line == NULL )
268  {
269  act_framecounter = 0;
270  return false;
271  }
272 
273  return true;
274 }
275 
276 
277 /*
278  * Parses a single line of timestamp data in one tracking data packet.
279  */
280 bool DTrackParser::parseLine_ts( char **line )
281 {
282  *line = string_get_d( *line, &act_timestamp );
283  if ( *line == NULL )
284  {
285  act_timestamp = -1;
286  return false;
287  }
288 
289  return true;
290 }
291 
292 
293 /*
294  * Parses a single line of extended timestamp data in one tracking data packet.
295  */
296 bool DTrackParser::parseLine_ts2( char **line )
297 {
298  *line = string_get_ui( *line, &act_timestamp_sec );
299 
300  if ( *line != NULL )
301  *line = string_get_ui( *line, &act_timestamp_usec );
302 
303  if ( *line != NULL )
304  *line = string_get_ui( *line, &act_latency_usec );
305 
306  if ( *line == NULL )
307  {
308  act_timestamp_sec = 0;
309  act_timestamp_usec = 0;
310  act_latency_usec = 0;
311  return false;
312  }
313 
314  act_timestamp = ( double )( act_timestamp_sec % ( 24 * 3600 ) ) + ( double )act_timestamp_usec / 1000000.0;
315 
316  return true;
317 }
318 
319 
320 /*
321  * Parses a single line of additional information about number of calibrated bodies in one tracking data packet.
322  */
323 bool DTrackParser::parseLine_6dcal( char **line )
324 {
325  *line = string_get_i( *line, &loc_num_bodycal );
326  if ( *line == NULL )
327  return false;
328 
329  return true;
330 }
331 
332 
333 /*
334  * Parses a single line of standard body data in one tracking data packet.
335  */
336 bool DTrackParser::parseLine_6d(char **line)
337 {
338  int i, j, n, id;
339  double d;
340 
341  // disable all existing data
342  for (i=0; i<act_num_body; i++) {
343  memset(&act_body[i], 0, sizeof(DTrack_Body_Type_d));
344  act_body[i].id = i;
345  act_body[i].quality = -1;
346  }
347 
348  // get number of standard bodies (in line)
349  *line = string_get_i( *line, &n );
350  if ( *line == NULL )
351  return false;
352 
353  // get data of standard bodies
354  for (i=0; i<n; i++) {
355  *line = string_get_block( *line, "id", &id, NULL, &d );
356  if ( *line == NULL )
357  return false;
358 
359  // adjust length of vector
360  if (id >= act_num_body) {
361  act_body.resize(id + 1);
362  for (j = act_num_body; j<=id; j++) {
363  memset(&act_body[j], 0, sizeof(DTrack_Body_Type_d));
364  act_body[j].id = j;
365  act_body[j].quality = -1;
366  }
367  act_num_body = id + 1;
368  }
369  act_body[id].id = id;
370  act_body[id].quality = d;
371 
372  *line = string_get_block( *line, "ddd", NULL, NULL, act_body[ id ].loc );
373  if ( *line == NULL )
374  return false;
375 
376  *line = string_get_block( *line, "ddddddddd", NULL, NULL, act_body[ id ].rot );
377  if ( *line == NULL )
378  return false;
379  }
380  return true;
381 }
382 
383 
384 /*
385  * Parses a single line of 6d covariance data in one tracking data packet.
386  */
387 bool DTrackParser::parseLine_6dcov(char **line)
388 {
389  int n, id;
390  double cov_reduced[21];
391 
392  // get number of standard bodies (in line)
393  *line = string_get_i( *line, &n );
394  if ( *line == NULL )
395  return false;
396 
397  // get covariance data
398  for ( int i = 0; i < n; i++ )
399  {
400  double covref[ 3 ];
401  *line = string_get_block( *line, "iddd", &id, NULL, covref );
402  if ( *line == NULL )
403  return false;
404 
405  for ( int j = 0; j < 3; j++ )
406  act_body[ id ].covref[ j ] = covref[ j ];
407 
408  *line = string_get_block( *line, "ddddddddddddddddddddd", NULL, NULL, cov_reduced );
409  if ( *line == NULL )
410  return false;
411 
412  reduced_to_full_cov( act_body[id].cov, cov_reduced, 6 );
413  }
414  return true;
415 }
416 
417 
418 /*
419  * Parses a single line of Flystick data (older format) data in one tracking data packet.
420  */
421 bool DTrackParser::parseLine_6df(char **line)
422 {
423  int i, j, k, n, iarr[2];
424  double d;
425 
426  // get number of calibrated Flysticks
427  *line = string_get_i( *line, &n );
428  if ( *line == NULL )
429  return false;
430 
431  loc_num_flystick1 = n;
432  // adjust length of vector
433  if (n != act_num_flystick) {
434  act_flystick.resize(n);
435  act_num_flystick = n;
436  }
437  // get data of Flysticks
438  for (i=0; i<n; i++) {
439  *line = string_get_block( *line, "idi", iarr, NULL, &d );
440  if ( *line == NULL )
441  return false;
442 
443  if (iarr[0] != i) { // not expected
444  return false;
445  }
446  act_flystick[i].id = iarr[0];
447  act_flystick[i].quality = d;
448  act_flystick[i].num_button = 8;
449  k = iarr[1];
450  for (j=0; j<8; j++) {
451  act_flystick[i].button[j] = k & 0x01;
452  k >>= 1;
453  }
454  act_flystick[i].num_joystick = 2; // additionally to buttons 5-8
455  if (iarr[1] & 0x20) {
456  act_flystick[i].joystick[0] = -1;
457  } else
458  if (iarr[1] & 0x80) {
459  act_flystick[i].joystick[0] = 1;
460  } else {
461  act_flystick[i].joystick[0] = 0;
462  }
463  if(iarr[1] & 0x10){
464  act_flystick[i].joystick[1] = -1;
465  }else if(iarr[1] & 0x40){
466  act_flystick[i].joystick[1] = 1;
467  }else{
468  act_flystick[i].joystick[1] = 0;
469  }
470 
471  *line = string_get_block( *line, "ddd", NULL, NULL, act_flystick[ i ].loc );
472  if ( *line == NULL )
473  return false;
474 
475  *line = string_get_block( *line, "ddddddddd", NULL, NULL, act_flystick[ i ].rot );
476  if ( *line == NULL )
477  return false;
478  }
479 
480  return true;
481 }
482 
483 
484 /*
485  * Parses a single line of Flystick data (newer format) data in one tracking data packet.
486  */
487 bool DTrackParser::parseLine_6df2(char **line)
488 {
489  int i, j, k, l, n, iarr[3];
490  double d;
491  char sfmt[20];
492 
493  // get number of calibrated Flysticks
494  *line = string_get_i( *line, &n );
495  if ( *line == NULL )
496  return false;
497 
498  // adjust length of vector
499  if (n != act_num_flystick) {
500  act_flystick.resize(n);
501  act_num_flystick = n;
502  }
503 
504  // get number of Flysticks
505  *line = string_get_i( *line, &n );
506  if ( *line == NULL )
507  return false;
508 
509  // get data of Flysticks
510  for (i=0; i<n; i++) {
511  *line = string_get_block( *line, "idii", iarr, NULL, &d );
512  if ( *line == NULL )
513  return false;
514 
515  if (iarr[0] != i) { // not expected
516  return false;
517  }
518  act_flystick[i].id = iarr[0];
519  act_flystick[i].quality = d;
520  if ( ( iarr[ 1 ] > DTRACKSDK_FLYSTICK_MAX_BUTTON ) ||( iarr[ 2 ] > DTRACKSDK_FLYSTICK_MAX_JOYSTICK ) )
521  {
522  return false;
523  }
524  act_flystick[i].num_button = iarr[1];
525  act_flystick[i].num_joystick = iarr[2];
526 
527  *line = string_get_block( *line, "ddd", NULL, NULL, act_flystick[ i ].loc );
528  if ( *line == NULL )
529  return false;
530 
531  *line = string_get_block( *line, "ddddddddd", NULL, NULL, act_flystick[ i ].rot );
532  if ( *line == NULL )
533  return false;
534 
535  strcpy_s( sfmt, sizeof( sfmt ), "" );
536  j = 0;
537  while ( j < act_flystick[ i ].num_button )
538  {
539  strcat_s( sfmt, sizeof( sfmt ), "i" );
540  j += 32;
541  }
542  j = 0;
543  while ( j < act_flystick[ i ].num_joystick )
544  {
545  strcat_s( sfmt, sizeof( sfmt ), "d" );
546  j++;
547  }
548 
549  *line = string_get_block( *line, sfmt, iarr, NULL, act_flystick[ i ].joystick );
550  if ( *line == NULL )
551  return false;
552 
553  k = l = 0;
554  for (j=0; j<act_flystick[i].num_button; j++) {
555  act_flystick[i].button[j] = iarr[k] & 0x01;
556  iarr[k] >>= 1;
557  l++;
558  if (l == 32) {
559  k++;
560  l = 0;
561  }
562  }
563  }
564 
565  return true;
566 }
567 
568 
569 /*
570  * Parses a single line of Measurement Tool data (older format) in one tracking data packet.
571  */
572 bool DTrackParser::parseLine_6dmt(char **line)
573 {
574  int i, j, k, n, iarr[3];
575  double d;
576 
577  // get number of calibrated measurement tools
578  *line = string_get_i( *line, &n );
579  if ( *line == NULL )
580  return false;
581 
582  loc_num_meatool1 = n;
583  // adjust length of vector
584  if (n != act_num_meatool) {
585  act_meatool.resize(n);
586  act_num_meatool = n;
587  }
588  // get data of measurement tools
589  for (i=0; i<n; i++) {
590  *line = string_get_block( *line, "idi", iarr, NULL, &d );
591  if ( *line == NULL )
592  return false;
593 
594  if (iarr[0] != i) { // not expected
595  return false;
596  }
597  act_meatool[i].id = iarr[0];
598  act_meatool[i].quality = d;
599 
600  act_meatool[i].num_button = 4;
601 
602  k = iarr[1];
603  for (j=0; j<act_meatool[i].num_button; j++) {
604  act_meatool[i].button[j] = k & 0x01;
605  k >>= 1;
606  }
607  for (j=act_meatool[i].num_button; j<DTRACKSDK_MEATOOL_MAX_BUTTON; j++) {
608  act_meatool[i].button[j] = 0;
609  }
610 
611  act_meatool[i].tipradius = 0.0;
612 
613  *line = string_get_block( *line, "ddd", NULL, NULL, act_meatool[ i ].loc );
614  if ( *line == NULL )
615  return false;
616 
617  *line = string_get_block( *line, "ddddddddd", NULL, NULL, act_meatool[ i ].rot );
618  if ( *line == NULL )
619  return false;
620 
621  for (j=0; j<9; j++)
622  act_meatool[i].cov[j] = 0.0;
623  }
624 
625  return true;
626 }
627 
628 
629 /*
630  * Parses a single line of Measurement Tool data (newer format) data in one tracking data packet.
631  */
632 bool DTrackParser::parseLine_6dmt2(char **line)
633 {
634  int i, j, k, l, n, iarr[2];
635  double darr[2];
636  char sfmt[20];
637  double cov_reduced[6];
638 
639  // get number of calibrated measurement tools
640  *line = string_get_i( *line, &n );
641  if ( *line == NULL )
642  return false;
643 
644  *line = string_get_i( *line, &n );
645  if ( *line == NULL )
646  return false;
647 
648  // adjust length of vector
649  if (n != act_num_meatool) {
650  act_meatool.resize(n);
651  act_num_meatool = n;
652  }
653  // get data of measurement tools
654  for (i=0; i<n; i++) {
655  *line = string_get_block( *line, "idid", iarr, NULL, darr );
656  if ( *line == NULL )
657  return false;
658 
659  if (iarr[0] != i) { // not expected
660  return false;
661  }
662  act_meatool[i].id = iarr[0];
663  act_meatool[i].quality = darr[0];
664 
665  act_meatool[i].num_button = iarr[1];
666  if (act_meatool[i].num_button > DTRACKSDK_MEATOOL_MAX_BUTTON)
667  act_meatool[i].num_button = DTRACKSDK_MEATOOL_MAX_BUTTON;
668 
669  for (j=act_meatool[i].num_button; j<DTRACKSDK_MEATOOL_MAX_BUTTON; j++) {
670  act_meatool[i].button[j] = 0;
671  }
672 
673  act_meatool[i].tipradius = darr[1];
674 
675  *line = string_get_block( *line, "ddd", NULL, NULL, act_meatool[ i ].loc );
676  if ( *line == NULL )
677  return false;
678 
679  *line = string_get_block( *line, "ddddddddd", NULL, NULL, act_meatool[ i ].rot );
680  if ( *line == NULL )
681  return false;
682 
683  strcpy_s( sfmt, sizeof( sfmt ), "" );
684  j = 0;
685  while ( j < act_meatool[ i ].num_button )
686  {
687  strcat_s( sfmt, sizeof( sfmt ), "i" );
688  j += 32;
689  }
690 
691  *line = string_get_block( *line, sfmt, iarr, NULL, NULL );
692  if ( *line == NULL )
693  return false;
694 
695  k = l = 0;
696  for (j=0; j<act_meatool[i].num_button; j++) {
697  act_meatool[i].button[j] = iarr[k] & 0x01;
698  iarr[k] >>= 1;
699  l++;
700  if (l == 32) {
701  k++;
702  l = 0;
703  }
704  }
705 
706  *line = string_get_block( *line, "dddddd", NULL, NULL, cov_reduced );
707  if ( *line == NULL )
708  return false;
709 
710  reduced_to_full_cov( act_meatool[i].cov, cov_reduced, 3 );
711  }
712 
713  return true;
714 }
715 
716 
717 /*
718  * Parses a single line of Measurement Tool reference data in one tracking data packet.
719  */
720 bool DTrackParser::parseLine_6dmtr(char **line)
721 {
722  int i, n, id;
723  double d;
724 
725  // get number of measurement references
726  *line = string_get_i( *line, &n );
727  if ( *line == NULL )
728  return false;
729 
730  // adjust length of vector
731  if (n != act_num_mearef) {
732  act_mearef.resize(n);
733  act_num_mearef = n;
734  }
735 
736  // reset data
737  for (i=0; i<n; i++)
738  {
739  act_mearef[i].id = i;
740  act_mearef[i].quality = -1;
741  }
742 
743  // get number of calibrated measurement references
744  *line = string_get_i( *line, &n );
745  if ( *line == NULL )
746  return false;
747 
748  // get data of measurement references
749  for (i=0; i<n; i++) {
750  *line = string_get_block( *line, "id", &id, NULL, &d );
751  if ( *line == NULL )
752  return false;
753 
754  if (id < 0 || id >= (int)act_mearef.size()) {
755  return false;
756  }
757  act_mearef[id].quality = d;
758 
759  *line = string_get_block( *line, "ddd", NULL, NULL, act_mearef[ id ].loc );
760  if ( *line == NULL )
761  return false;
762 
763  *line = string_get_block( *line, "ddddddddd", NULL, NULL, act_mearef[ id ].rot );
764  if ( *line == NULL )
765  return false;
766  }
767 
768  return true;
769 }
770 
771 
772 /*
773  * Parses a single line of additional information about number of calibrated A.R.T. FINGERTRACKING hands in one tracking data packet.
774  */
775 bool DTrackParser::parseLine_glcal(char **line)
776 {
777  *line = string_get_i( *line, &loc_num_handcal ); // get number of calibrated hands
778  if ( *line == NULL )
779  return false;
780 
781  return true;
782 }
783 
784 
785 /*
786  * Parses a single line of A.R.T. FINGERTRACKING hand data in one tracking data packet.
787  */
788 bool DTrackParser::parseLine_gl(char **line)
789 {
790  int i, j, n, iarr[3], id;
791  double d, darr[6];
792 
793  // disable all existing data
794  for (i=0; i<act_num_hand; i++) {
795  memset(&act_hand[i], 0, sizeof(DTrack_Hand_Type_d));
796  act_hand[i].id = i;
797  act_hand[i].quality = -1;
798  }
799 
800  // get number of hands (in line)
801  *line = string_get_i( *line, &n );
802  if ( *line == NULL )
803  return false;
804 
805  // get data of hands
806  for (i=0; i<n; i++) {
807  *line = string_get_block( *line, "idii", iarr, NULL, &d );
808  if ( *line == NULL )
809  return false;
810 
811  id = iarr[0];
812  if (id >= act_num_hand) { // adjust length of vector
813  act_hand.resize(id + 1);
814  for (j=act_num_hand; j<=id; j++) {
815  memset(&act_hand[j], 0, sizeof(DTrack_Hand_Type_d));
816  act_hand[j].id = j;
817  act_hand[j].quality = -1;
818  }
819  act_num_hand = id + 1;
820  }
821  act_hand[id].id = iarr[0];
822  act_hand[id].lr = iarr[1];
823  act_hand[id].quality = d;
824  if (iarr[2] > DTRACKSDK_HAND_MAX_FINGER) {
825  return false;
826  }
827  act_hand[id].nfinger = iarr[2];
828 
829  *line = string_get_block( *line, "ddd", NULL, NULL, act_hand[ id ].loc );
830  if ( *line == NULL )
831  return false;
832 
833  *line = string_get_block( *line, "ddddddddd", NULL, NULL, act_hand[ id ].rot );
834  if ( *line == NULL )
835  return false;
836 
837  // get data of fingers
838  for (j = 0; j < act_hand[id].nfinger; j++) {
839  *line = string_get_block( *line, "ddd", NULL, NULL, act_hand[ id ].finger[ j ].loc );
840  if ( *line == NULL )
841  return false;
842 
843  *line = string_get_block( *line, "ddddddddd", NULL, NULL, act_hand[ id ].finger[ j ].rot );
844  if ( *line == NULL )
845  return false;
846 
847  *line = string_get_block( *line, "dddddd", NULL, NULL, darr );
848  if ( *line == NULL )
849  return false;
850 
851  act_hand[id].finger[j].radiustip = darr[0];
852  act_hand[id].finger[j].lengthphalanx[0] = darr[1];
853  act_hand[id].finger[j].anglephalanx[0] = darr[2];
854  act_hand[id].finger[j].lengthphalanx[1] = darr[3];
855  act_hand[id].finger[j].anglephalanx[1] = darr[4];
856  act_hand[id].finger[j].lengthphalanx[2] = darr[5];
857  }
858  }
859 
860  return true;
861 }
862 
863 
864 /*
865  * Parses a single line of ART-Human model data in one tracking data packet.
866  */
867 bool DTrackParser::parseLine_6dj(char **line)
868 {
869  int i, j, n, iarr[2], id;
870  double d, darr[6];
871 
872  // get number of calibrated human models
873  *line = string_get_i( *line, &n );
874  if ( *line == NULL )
875  return false;
876 
877  // adjust length of vector
878  if(n != act_num_human){
879  act_human.resize(n);
880  act_num_human = n;
881  }
882  for(i=0; i<act_num_human; i++){
883  memset(&act_human[i], 0, sizeof(DTrack_Human_Type_d));
884  act_human[i].id = i;
885  act_human[i].num_joints = 0;
886  }
887 
888  // get number of human models
889  *line = string_get_i( *line, &n );
890  if ( *line == NULL )
891  return false;
892 
893  int id_human;
894  for (i=0; i<n; i++) {
895  *line = string_get_block( *line, "ii", iarr, NULL,NULL );
896  if ( *line == NULL )
897  return false;
898 
899  if (iarr[0] > act_num_human - 1) // not expected
900  return false;
901 
902  id_human = iarr[0];
903  act_human[id_human].id = iarr[0];
904  act_human[id_human].num_joints = iarr[1];
905 
906  for (j = 0; j < iarr[1]; j++){
907  *line = string_get_block( *line, "id", &id, NULL, &d );
908  if ( *line == NULL )
909  return false;
910 
911  act_human[id_human].joint[j].id = id;
912  act_human[id_human].joint[j].quality = d;
913 
914  *line = string_get_block( *line, "dddddd", NULL, NULL, darr );
915  if ( *line == NULL )
916  return false;
917 
918  memcpy(act_human[id_human].joint[j].loc, &darr, 3*sizeof(double));
919  memcpy(act_human[id_human].joint[j].ang, &darr[3], 3*sizeof(double));
920 
921  *line = string_get_block( *line, "ddddddddd", NULL, NULL, act_human[ id_human ].joint[ j ].rot );
922  if ( *line == NULL )
923  return false;
924  }
925  }
926 
927  return true;
928 }
929 
930 
931 /*
932  * Parses a single line of hybrid (optical-inertial) body data in one tracking data packet.
933  */
934 bool DTrackParser::parseLine_6di(char **line)
935 {
936  int i, j, n, iarr[2], id, st;
937  double d;
938 
939  // disable all existing data
940  for (i=0; i<act_num_inertial; i++) {
941  memset(&act_inertial[i], 0, sizeof(DTrack_Inertial_Type_d));
942  act_inertial[i].id = i;
943  act_inertial[i].st = 0;
944  act_inertial[i].error = 0;
945  }
946 
947  // get number of calibrated inertial bodies
948  *line = string_get_i( *line, &n );
949  if ( *line == NULL )
950  return false;
951 
952  // get data of inertial bodies
953  for (i=0; i<n; i++) {
954  *line = string_get_block( *line, "iid", iarr, NULL, &d );
955  if ( *line == NULL )
956  return false;
957 
958  id = iarr[0];
959  st = iarr[1];
960  // adjust length of vector
961  if (id >= act_num_inertial) {
962  act_inertial.resize(id + 1);
963  for (j = act_num_inertial; j<=id; j++) {
964  memset(&act_inertial[j], 0, sizeof(DTrack_Inertial_Type_d));
965  act_inertial[ j ].id = j;
966  act_inertial[ j ].st = 0;
967  act_inertial[ j ].error = 0;
968  }
969  act_num_inertial = id + 1;
970  }
971  act_inertial[id].id = id;
972  act_inertial[id].st = st;
973  act_inertial[id].error = d;
974 
975  *line = string_get_block( *line, "ddd", NULL, NULL, act_inertial[ id ].loc );
976  if ( *line == NULL )
977  return false;
978 
979  *line = string_get_block( *line, "ddddddddd", NULL, NULL, act_inertial[ id ].rot );
980  if ( *line == NULL )
981  return false;
982  }
983 
984  return true;
985 }
986 
987 
988 /*
989  * Parses a single line of single marker data in one tracking data packet.
990  */
991 bool DTrackParser::parseLine_3d( char **line )
992 {
993  // get number of markers
994  *line = string_get_i( *line, &act_num_marker );
995  if ( *line == NULL )
996  {
997  act_num_marker = 0;
998  return false;
999  }
1000  if ( act_num_marker > ( int )act_marker.size() )
1001  {
1002  act_marker.resize( act_num_marker );
1003  }
1004 
1005  // get data of single markers
1006  for ( int i = 0; i < act_num_marker; i++ )
1007  {
1008  *line = string_get_block( *line, "id", &act_marker[ i ].id, NULL, &act_marker[ i ].quality );
1009  if ( *line == NULL )
1010  return false;
1011 
1012  *line = string_get_block( *line, "ddd", NULL, NULL, act_marker[ i ].loc );
1013  if ( *line == NULL )
1014  return false;
1015  }
1016 
1017  return true;
1018 }
1019 
1020 
1021 /*
1022  * Parses a single line of system status data in one tracking data packet.
1023  */
1024 bool DTrackParser::parseLine_st( char** line )
1025 {
1026  int ngrp, id, ncam, nval;
1027  int iarr[ 5 ];
1028 
1029  // get number of following block groups
1030  *line = string_get_i( *line, &ngrp );
1031  if ( *line == NULL )
1032  return false;
1033 
1034  if ( ngrp > 3 ) ngrp = 3; // ignore additional (i.e. future) status types
1035 
1036  // caution: expects exactly three status types in order of their ID number
1037 
1038  for ( int igrp = 0; igrp < ngrp; igrp++ )
1039  {
1040  // get description of status type
1041  if ( igrp == 2 ) // status type '2' has an additional value (number of cameras)
1042  {
1043  *line = string_get_block( *line, "iii", iarr, NULL, NULL );
1044  if ( *line == NULL )
1045  return false;
1046 
1047  id = iarr[ 0 ];
1048  ncam = iarr[ 1 ];
1049  nval = iarr[ 2 ];
1050  }
1051  else // for status types '0' and '1'
1052  {
1053  *line = string_get_block( *line, "ii", iarr, NULL, NULL );
1054  if ( *line == NULL )
1055  return false;
1056 
1057  id = iarr[ 0 ];
1058  ncam = 0;
1059  nval = iarr[ 1 ];
1060  }
1061 
1062  // get values of status type
1063  if ( id == 0 ) // general status values
1064  {
1065  if ( nval < 3 ) return false; // more sophisticated at future enhancements of status values
1066 
1067  *line = string_get_block( *line, "iii", iarr, NULL, NULL );
1068  if ( *line == NULL )
1069  return false;
1070 
1071  act_status.numCameras = iarr[ 0 ];
1072  act_status.numTrackedBodies = iarr[ 1 ];
1073  act_status.numTrackedMarkers = iarr[ 2 ];
1074  }
1075  else if ( id == 1 ) // message statistics
1076  {
1077  if ( nval < 5 ) return false; // more sophisticated at future enhancements of status values
1078 
1079  *line = string_get_block( *line, "iiiii", iarr, NULL, NULL );
1080  if ( *line == NULL )
1081  return false;
1082 
1083  act_status.numCameraErrorMessages = iarr[ 0 ];
1084  act_status.numCameraWarningMessages = iarr[ 1 ];
1085  act_status.numOtherErrorMessages = iarr[ 2 ];
1086  act_status.numOtherWarningMessages = iarr[ 3 ];
1087  act_status.numInfoMessages = iarr[ 4 ];
1088  }
1089  else if ( id == 2 ) // camera status values
1090  {
1091  if ( nval < 3 ) return false; // more sophisticated at future enhancements of status values
1092 
1093  // adjust length of vector
1094  act_status.cameraStatus.resize( ncam );
1095 
1096  for ( int icam = 0; icam < ncam; icam++ )
1097  {
1098  *line = string_get_block( *line, "iiii", iarr, NULL, NULL );
1099  if ( *line == NULL )
1100  return false;
1101 
1102  act_status.cameraStatus[ icam ].idCamera = iarr[ 0 ];
1103  act_status.cameraStatus[ icam ].numReflections = iarr[ 1 ];
1104  act_status.cameraStatus[ icam ].numReflectionsUsed = iarr[ 2 ];
1105  act_status.cameraStatus[ icam ].maxIntensity = iarr[ 3 ];
1106  }
1107  }
1108  }
1109 
1110  act_is_status_available = true;
1111  return true;
1112 }
1113 
1114 
1115 /*
1116  * Get number of calibrated standard bodies (as far as known).
1117  */
1119 {
1120  return act_num_body;
1121 }
1122 
1123 
1124 /*
1125  * Get standard body data.
1126  */
1127 const DTrackBody* DTrackParser::getBody( int id ) const
1128 {
1129  if ((id >= 0) && (id < act_num_body))
1130  return &act_body.at(id);
1131  return NULL;
1132 }
1133 
1134 
1135 /*
1136  * Get number of calibrated Flysticks.
1137  */
1139 {
1140  return act_num_flystick;
1141 }
1142 
1143 
1144 /*
1145  * Get Flystick data.
1146  */
1148 {
1149  if ((id >= 0) && (id < act_num_flystick))
1150  return &act_flystick.at(id);
1151  return NULL;
1152 }
1153 
1154 
1155 /*
1156  * Get number of calibrated Measurement Tools.
1157  */
1159 {
1160  return act_num_meatool;
1161 }
1162 
1163 
1164 /*
1165  * Get Measurement Tool data.
1166  */
1168 {
1169  if ((id >= 0) && (id < act_num_meatool))
1170  return &act_meatool.at(id);
1171  return NULL;
1172 }
1173 
1174 
1175 /*
1176  * Get number of calibrated Measurement Tool references.
1177  */
1179 {
1180  return act_num_mearef;
1181 }
1182 
1183 
1184 /*
1185  * Get Measurement Tool reference data.
1186  */
1188 {
1189  if ((id >= 0) && (id < act_num_mearef))
1190  return &act_mearef.at(id);
1191  return NULL;
1192 }
1193 
1194 
1195 /*
1196  * Get number of calibrated A.R.T. FINGERTRACKING hands (as far as known).
1197  */
1199 {
1200  return act_num_hand;
1201 }
1202 
1203 
1204 /*
1205  * Get A.R.T. FINGERTRACKING hand data.
1206  */
1207 const DTrackHand* DTrackParser::getHand( int id ) const
1208 {
1209  if ((id >= 0) && (id < act_num_hand))
1210  return &act_hand.at(id);
1211  return NULL;
1212 }
1213 
1214 
1215 /*
1216  * Get number of calibrated ART-Human models.
1217  */
1219 {
1220  return act_num_human;
1221 }
1222 
1223 
1224 /*
1225  * Get ART-Human model data.
1226  */
1227 const DTrackHuman* DTrackParser::getHuman( int id ) const
1228 {
1229  if ((id >= 0) && (id < act_num_human))
1230  return &act_human.at(id);
1231  return NULL;
1232 }
1233 
1234 
1235 /*
1236  * Get number of calibrated hybrid (optical-inertial) bodies.
1237  */
1239 {
1240  return act_num_inertial;
1241 }
1242 
1243 
1244 /*
1245  * Get hybrid (optical-inertial) data.
1246 */
1248 {
1249  if((id >=0) && (id < act_num_inertial))
1250  return &act_inertial.at(id);
1251  return NULL;
1252 }
1253 
1254 
1255 /*
1256  * Get number of tracked single markers.
1257  */
1259 {
1260  return act_num_marker;
1261 }
1262 
1263 
1264 /*
1265  * Get single marker data.
1266  */
1267 const DTrackMarker* DTrackParser::getMarker( int index ) const
1268 {
1269  if ((index >= 0) && (index < act_num_marker))
1270  return &act_marker.at(index);
1271  return NULL;
1272 }
1273 
1274 
1275 /*
1276  * Get frame counter.
1277  */
1278 unsigned int DTrackParser::getFrameCounter() const
1279 {
1280  return act_framecounter;
1281 }
1282 
1283 
1284 /*
1285  * Get timestamp since midnight.
1286  */
1288 {
1289  return act_timestamp;
1290 }
1291 
1292 
1293 /*
1294  * Get timestamp since Unix epoch, seconds.
1295  */
1296 unsigned int DTrackParser::getTimeStampSec() const
1297 {
1298  return act_timestamp_sec;
1299 }
1300 
1301 
1302 /*
1303  * Get timestamp since Unix epoch, microseconds.
1304  */
1306 {
1307  return act_timestamp_usec;
1308 }
1309 
1310 
1311 /*
1312  * Get latency.
1313  */
1314 unsigned int DTrackParser::getLatencyUsec() const
1315 {
1316  return act_latency_usec;
1317 }
1318 
1319 
1320 /*
1321  * Returns if system status data is available.
1322  */
1324 {
1325  return act_is_status_available;
1326 }
1327 
1328 
1329 /*
1330  * Get system status data.
1331  */
1333 {
1334  if ( ! act_is_status_available ) return NULL;
1335 
1336  return &act_status;
1337 }
1338 
double getTimeStamp() const
Get timestamp since midnight.
int getNumFlyStick() const
Get number of calibrated Flysticks.
const DTrackFlyStick * getFlyStick(int id) const
Get Flystick data.
const DTrackMeaTool * getMeaTool(int id) const
Get Measurement Tool data.
int getNumBody() const
Get number of calibrated standard bodies (as far as known).
bool isStatusAvailable() const
Returns if system status data is available.
DTrackParser()
Constructor.
Single marker data (3DOF).
int getNumHand() const
Get number of calibrated A.R.T. FINGERTRACKING hands (as far as known).
A.R.T. Flystick data (6DOF + buttons).
Hybrid (optical-inertial) body data (6DOF).
Measurement Tool data (6DOF + buttons).
A.R.T. FINGERTRACKING hand data (6DOF + fingers).
const DTrackBody * getBody(int id) const
Get standard body data.
const DTrackMeaRef * getMeaRef(int id) const
Get Measurement Tool reference data.
virtual ~DTrackParser()
Destructor.
const DTrackMarker * getMarker(int index) const
Get single marker data.
unsigned int getLatencyUsec() const
Get latency (delay between exposure and sending UDP data in Controller).
int getNumMeaRef() const
Get number of calibrated Measurement Tool references.
int getNumMarker() const
Get number of tracked single markers.
unsigned int getFrameCounter() const
Get frame counter.
unsigned int getTimeStampUsec() const
Get timestamp since Unix epoch (1970-01-01 00:00:00), microseconds.
const DTrackHuman * getHuman(int id) const
Get ART-Human model data.
unsigned int getTimeStampSec() const
Get timestamp since Unix epoch (1970-01-01 00:00:00), seconds.
const DTrackHand * getHand(int id) const
Get A.R.T. FINGERTRACKING hand data.
const DTrackInertial * getInertial(int id) const
Get hybrid (optical-inertial) data.
int getNumInertial() const
Get number of calibrated hybrid (optical-inertial) bodies.
const DTrackStatus * getStatus() const
Get system status data.
bool parseLine(char **line)
Parses a single line of data in one tracking data packet.
Measurement Tool reference data (6DOF).
void startFrame()
Set default values at start of a new frame.
int getNumHuman() const
Get number of calibrated ART-Human models.
Standard body data (6DOF).
ART-Human model (joints (6DOF) including optional Fingertracking).
int getNumMeaTool() const
Get number of calibrated Measurement Tools.
void endFrame()
Final adjustments after processing all data for a frame.