35 #include "DTrackParser.hpp" 36 #include "DTrackParse.hpp" 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 49 static void reduced_to_full_cov(
double* cov_full,
const double* cov_reduced,
int dim )
51 for (
int r=0; r<dim; ++r) {
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 ];
69 act_timestamp_sec = 0;
70 act_timestamp_usec = 0;
73 act_num_body = act_num_flystick = act_num_meatool = act_num_mearef = act_num_hand = act_num_human = 0;
77 act_is_status_available =
false;
97 act_timestamp_sec = 0;
98 act_timestamp_usec = 0;
100 act_is_status_available =
false;
102 loc_num_bodycal = loc_num_handcal = -1;
103 loc_num_flystick1 = loc_num_meatool1 = 0;
115 if (loc_num_bodycal >= 0) {
116 n = loc_num_bodycal - loc_num_flystick1 - loc_num_meatool1;
117 if (n > act_num_body) {
119 for (j=act_num_body; j<n; j++) {
122 act_body[j].quality = -1;
129 if (loc_num_handcal >= 0) {
130 if (loc_num_handcal > act_num_hand) {
131 act_hand.resize(loc_num_handcal);
132 for (j=act_num_hand; j<loc_num_handcal; j++) {
135 act_hand[j].quality = -1;
138 act_num_hand = loc_num_handcal;
152 if (!strncmp(*line,
"fr ", 3)) {
154 return parseLine_fr(line);
158 if ( strncmp( *line,
"ts ", 3 ) == 0 )
161 return parseLine_ts( line );
165 if ( strncmp( *line,
"ts2 ", 4 ) == 0 )
168 return parseLine_ts2( line );
172 if ( strncmp( *line,
"6dcal ", 6 ) == 0 )
175 return parseLine_6dcal( line );
179 if (!strncmp(*line,
"6d ", 3)) {
181 return parseLine_6d(line);
185 if (!strncmp(*line,
"6dcov ", 6)) {
187 return parseLine_6dcov(line);
191 if (!strncmp(*line,
"6df ", 4)) {
193 return parseLine_6df(line);
197 if (!strncmp(*line,
"6df2 ", 5)) {
199 return parseLine_6df2(line);
203 if (!strncmp(*line,
"6dmt ", 5)) {
205 return parseLine_6dmt(line);
209 if (!strncmp(*line,
"6dmt2 ", 6)) {
211 return parseLine_6dmt2(line);
215 if (!strncmp(*line,
"6dmtr ", 6)) {
217 return parseLine_6dmtr(line);
221 if (!strncmp(*line,
"glcal ", 6)) {
223 return parseLine_glcal(line);
227 if (!strncmp(*line,
"gl ", 3)) {
229 return parseLine_gl(line);
233 if (!strncmp(*line,
"6dj ", 4)) {
235 return parseLine_6dj(line);
239 if (!strncmp(*line,
"6di ", 4)) {
241 return parseLine_6di(line);
245 if (!strncmp(*line,
"3d ", 3)) {
247 return parseLine_3d(line);
251 if ( strncmp( *line,
"st ", 3 ) == 0 )
254 return parseLine_st( line );
264 bool DTrackParser::parseLine_fr(
char **line)
266 *line = string_get_ui( *line, &act_framecounter );
269 act_framecounter = 0;
280 bool DTrackParser::parseLine_ts(
char **line )
282 *line = string_get_d( *line, &act_timestamp );
296 bool DTrackParser::parseLine_ts2(
char **line )
298 *line = string_get_ui( *line, &act_timestamp_sec );
301 *line = string_get_ui( *line, &act_timestamp_usec );
304 *line = string_get_ui( *line, &act_latency_usec );
308 act_timestamp_sec = 0;
309 act_timestamp_usec = 0;
310 act_latency_usec = 0;
314 act_timestamp = ( double )( act_timestamp_sec % ( 24 * 3600 ) ) + (
double )act_timestamp_usec / 1000000.0;
323 bool DTrackParser::parseLine_6dcal(
char **line )
325 *line = string_get_i( *line, &loc_num_bodycal );
336 bool DTrackParser::parseLine_6d(
char **line)
342 for (i=0; i<act_num_body; i++) {
345 act_body[i].quality = -1;
349 *line = string_get_i( *line, &n );
354 for (i=0; i<n; i++) {
355 *line = string_get_block( *line,
"id", &
id, NULL, &d );
360 if (
id >= act_num_body) {
361 act_body.resize(
id + 1);
362 for (j = act_num_body; j<=id; j++) {
365 act_body[j].quality = -1;
367 act_num_body =
id + 1;
369 act_body[id].id = id;
370 act_body[id].quality = d;
372 *line = string_get_block( *line,
"ddd", NULL, NULL, act_body[
id ].loc );
376 *line = string_get_block( *line,
"ddddddddd", NULL, NULL, act_body[
id ].rot );
387 bool DTrackParser::parseLine_6dcov(
char **line)
390 double cov_reduced[21];
393 *line = string_get_i( *line, &n );
398 for (
int i = 0; i < n; i++ )
401 *line = string_get_block( *line,
"iddd", &
id, NULL, covref );
405 for (
int j = 0; j < 3; j++ )
406 act_body[
id ].covref[ j ] = covref[ j ];
408 *line = string_get_block( *line,
"ddddddddddddddddddddd", NULL, NULL, cov_reduced );
412 reduced_to_full_cov( act_body[
id].cov, cov_reduced, 6 );
421 bool DTrackParser::parseLine_6df(
char **line)
423 int i, j, k, n, iarr[2];
427 *line = string_get_i( *line, &n );
431 loc_num_flystick1 = n;
433 if (n != act_num_flystick) {
434 act_flystick.resize(n);
435 act_num_flystick = n;
438 for (i=0; i<n; i++) {
439 *line = string_get_block( *line,
"idi", iarr, NULL, &d );
446 act_flystick[i].id = iarr[0];
447 act_flystick[i].quality = d;
448 act_flystick[i].num_button = 8;
450 for (j=0; j<8; j++) {
451 act_flystick[i].button[j] = k & 0x01;
454 act_flystick[i].num_joystick = 2;
455 if (iarr[1] & 0x20) {
456 act_flystick[i].joystick[0] = -1;
458 if (iarr[1] & 0x80) {
459 act_flystick[i].joystick[0] = 1;
461 act_flystick[i].joystick[0] = 0;
464 act_flystick[i].joystick[1] = -1;
465 }
else if(iarr[1] & 0x40){
466 act_flystick[i].joystick[1] = 1;
468 act_flystick[i].joystick[1] = 0;
471 *line = string_get_block( *line,
"ddd", NULL, NULL, act_flystick[ i ].loc );
475 *line = string_get_block( *line,
"ddddddddd", NULL, NULL, act_flystick[ i ].rot );
487 bool DTrackParser::parseLine_6df2(
char **line)
489 int i, j, k, l, n, iarr[3];
494 *line = string_get_i( *line, &n );
499 if (n != act_num_flystick) {
500 act_flystick.resize(n);
501 act_num_flystick = n;
505 *line = string_get_i( *line, &n );
510 for (i=0; i<n; i++) {
511 *line = string_get_block( *line,
"idii", iarr, NULL, &d );
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 ) )
524 act_flystick[i].num_button = iarr[1];
525 act_flystick[i].num_joystick = iarr[2];
527 *line = string_get_block( *line,
"ddd", NULL, NULL, act_flystick[ i ].loc );
531 *line = string_get_block( *line,
"ddddddddd", NULL, NULL, act_flystick[ i ].rot );
535 strcpy_s( sfmt,
sizeof( sfmt ),
"" );
537 while ( j < act_flystick[ i ].num_button )
539 strcat_s( sfmt,
sizeof( sfmt ),
"i" );
543 while ( j < act_flystick[ i ].num_joystick )
545 strcat_s( sfmt,
sizeof( sfmt ),
"d" );
549 *line = string_get_block( *line, sfmt, iarr, NULL, act_flystick[ i ].joystick );
554 for (j=0; j<act_flystick[i].num_button; j++) {
555 act_flystick[i].button[j] = iarr[k] & 0x01;
572 bool DTrackParser::parseLine_6dmt(
char **line)
574 int i, j, k, n, iarr[3];
578 *line = string_get_i( *line, &n );
582 loc_num_meatool1 = n;
584 if (n != act_num_meatool) {
585 act_meatool.resize(n);
589 for (i=0; i<n; i++) {
590 *line = string_get_block( *line,
"idi", iarr, NULL, &d );
597 act_meatool[i].id = iarr[0];
598 act_meatool[i].quality = d;
600 act_meatool[i].num_button = 4;
603 for (j=0; j<act_meatool[i].num_button; j++) {
604 act_meatool[i].button[j] = k & 0x01;
607 for (j=act_meatool[i].num_button; j<DTRACKSDK_MEATOOL_MAX_BUTTON; j++) {
608 act_meatool[i].button[j] = 0;
611 act_meatool[i].tipradius = 0.0;
613 *line = string_get_block( *line,
"ddd", NULL, NULL, act_meatool[ i ].loc );
617 *line = string_get_block( *line,
"ddddddddd", NULL, NULL, act_meatool[ i ].rot );
622 act_meatool[i].cov[j] = 0.0;
632 bool DTrackParser::parseLine_6dmt2(
char **line)
634 int i, j, k, l, n, iarr[2];
637 double cov_reduced[6];
640 *line = string_get_i( *line, &n );
644 *line = string_get_i( *line, &n );
649 if (n != act_num_meatool) {
650 act_meatool.resize(n);
654 for (i=0; i<n; i++) {
655 *line = string_get_block( *line,
"idid", iarr, NULL, darr );
662 act_meatool[i].id = iarr[0];
663 act_meatool[i].quality = darr[0];
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;
669 for (j=act_meatool[i].num_button; j<DTRACKSDK_MEATOOL_MAX_BUTTON; j++) {
670 act_meatool[i].button[j] = 0;
673 act_meatool[i].tipradius = darr[1];
675 *line = string_get_block( *line,
"ddd", NULL, NULL, act_meatool[ i ].loc );
679 *line = string_get_block( *line,
"ddddddddd", NULL, NULL, act_meatool[ i ].rot );
683 strcpy_s( sfmt,
sizeof( sfmt ),
"" );
685 while ( j < act_meatool[ i ].num_button )
687 strcat_s( sfmt,
sizeof( sfmt ),
"i" );
691 *line = string_get_block( *line, sfmt, iarr, NULL, NULL );
696 for (j=0; j<act_meatool[i].num_button; j++) {
697 act_meatool[i].button[j] = iarr[k] & 0x01;
706 *line = string_get_block( *line,
"dddddd", NULL, NULL, cov_reduced );
710 reduced_to_full_cov( act_meatool[i].cov, cov_reduced, 3 );
720 bool DTrackParser::parseLine_6dmtr(
char **line)
726 *line = string_get_i( *line, &n );
731 if (n != act_num_mearef) {
732 act_mearef.resize(n);
739 act_mearef[i].id = i;
740 act_mearef[i].quality = -1;
744 *line = string_get_i( *line, &n );
749 for (i=0; i<n; i++) {
750 *line = string_get_block( *line,
"id", &
id, NULL, &d );
754 if (id < 0 || id >= (
int)act_mearef.size()) {
757 act_mearef[id].quality = d;
759 *line = string_get_block( *line,
"ddd", NULL, NULL, act_mearef[
id ].loc );
763 *line = string_get_block( *line,
"ddddddddd", NULL, NULL, act_mearef[
id ].rot );
775 bool DTrackParser::parseLine_glcal(
char **line)
777 *line = string_get_i( *line, &loc_num_handcal );
788 bool DTrackParser::parseLine_gl(
char **line)
790 int i, j, n, iarr[3], id;
794 for (i=0; i<act_num_hand; i++) {
797 act_hand[i].quality = -1;
801 *line = string_get_i( *line, &n );
806 for (i=0; i<n; i++) {
807 *line = string_get_block( *line,
"idii", iarr, NULL, &d );
812 if (
id >= act_num_hand) {
813 act_hand.resize(
id + 1);
814 for (j=act_num_hand; j<=id; j++) {
817 act_hand[j].quality = -1;
819 act_num_hand =
id + 1;
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) {
827 act_hand[id].nfinger = iarr[2];
829 *line = string_get_block( *line,
"ddd", NULL, NULL, act_hand[
id ].loc );
833 *line = string_get_block( *line,
"ddddddddd", NULL, NULL, act_hand[
id ].rot );
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 );
843 *line = string_get_block( *line,
"ddddddddd", NULL, NULL, act_hand[
id ].finger[ j ].rot );
847 *line = string_get_block( *line,
"dddddd", NULL, NULL, darr );
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];
867 bool DTrackParser::parseLine_6dj(
char **line)
869 int i, j, n, iarr[2], id;
873 *line = string_get_i( *line, &n );
878 if(n != act_num_human){
882 for(i=0; i<act_num_human; i++){
885 act_human[i].num_joints = 0;
889 *line = string_get_i( *line, &n );
894 for (i=0; i<n; i++) {
895 *line = string_get_block( *line,
"ii", iarr, NULL,NULL );
899 if (iarr[0] > act_num_human - 1)
903 act_human[id_human].id = iarr[0];
904 act_human[id_human].num_joints = iarr[1];
906 for (j = 0; j < iarr[1]; j++){
907 *line = string_get_block( *line,
"id", &
id, NULL, &d );
911 act_human[id_human].joint[j].id = id;
912 act_human[id_human].joint[j].quality = d;
914 *line = string_get_block( *line,
"dddddd", NULL, NULL, darr );
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));
921 *line = string_get_block( *line,
"ddddddddd", NULL, NULL, act_human[ id_human ].joint[ j ].rot );
934 bool DTrackParser::parseLine_6di(
char **line)
936 int i, j, n, iarr[2], id, st;
940 for (i=0; i<act_num_inertial; i++) {
942 act_inertial[i].id = i;
943 act_inertial[i].st = 0;
944 act_inertial[i].error = 0;
948 *line = string_get_i( *line, &n );
953 for (i=0; i<n; i++) {
954 *line = string_get_block( *line,
"iid", iarr, NULL, &d );
961 if (
id >= act_num_inertial) {
962 act_inertial.resize(
id + 1);
963 for (j = act_num_inertial; j<=id; j++) {
965 act_inertial[ j ].id = j;
966 act_inertial[ j ].st = 0;
967 act_inertial[ j ].error = 0;
969 act_num_inertial =
id + 1;
971 act_inertial[id].id = id;
972 act_inertial[id].st = st;
973 act_inertial[id].error = d;
975 *line = string_get_block( *line,
"ddd", NULL, NULL, act_inertial[
id ].loc );
979 *line = string_get_block( *line,
"ddddddddd", NULL, NULL, act_inertial[
id ].rot );
991 bool DTrackParser::parseLine_3d(
char **line )
994 *line = string_get_i( *line, &act_num_marker );
1000 if ( act_num_marker > (
int )act_marker.size() )
1002 act_marker.resize( act_num_marker );
1006 for (
int i = 0; i < act_num_marker; i++ )
1008 *line = string_get_block( *line,
"id", &act_marker[ i ].
id, NULL, &act_marker[ i ].quality );
1009 if ( *line == NULL )
1012 *line = string_get_block( *line,
"ddd", NULL, NULL, act_marker[ i ].loc );
1013 if ( *line == NULL )
1024 bool DTrackParser::parseLine_st(
char** line )
1026 int ngrp, id, ncam, nval;
1030 *line = string_get_i( *line, &ngrp );
1031 if ( *line == NULL )
1034 if ( ngrp > 3 ) ngrp = 3;
1038 for (
int igrp = 0; igrp < ngrp; igrp++ )
1043 *line = string_get_block( *line,
"iii", iarr, NULL, NULL );
1044 if ( *line == NULL )
1053 *line = string_get_block( *line,
"ii", iarr, NULL, NULL );
1054 if ( *line == NULL )
1065 if ( nval < 3 )
return false;
1067 *line = string_get_block( *line,
"iii", iarr, NULL, NULL );
1068 if ( *line == NULL )
1071 act_status.numCameras = iarr[ 0 ];
1072 act_status.numTrackedBodies = iarr[ 1 ];
1073 act_status.numTrackedMarkers = iarr[ 2 ];
1077 if ( nval < 5 )
return false;
1079 *line = string_get_block( *line,
"iiiii", iarr, NULL, NULL );
1080 if ( *line == NULL )
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 ];
1091 if ( nval < 3 )
return false;
1094 act_status.cameraStatus.resize( ncam );
1096 for (
int icam = 0; icam < ncam; icam++ )
1098 *line = string_get_block( *line,
"iiii", iarr, NULL, NULL );
1099 if ( *line == NULL )
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 ];
1110 act_is_status_available =
true;
1120 return act_num_body;
1129 if ((
id >= 0) && (
id < act_num_body))
1130 return &act_body.at(
id);
1140 return act_num_flystick;
1149 if ((
id >= 0) && (
id < act_num_flystick))
1150 return &act_flystick.at(
id);
1160 return act_num_meatool;
1169 if ((
id >= 0) && (
id < act_num_meatool))
1170 return &act_meatool.at(
id);
1180 return act_num_mearef;
1189 if ((
id >= 0) && (
id < act_num_mearef))
1190 return &act_mearef.at(
id);
1200 return act_num_hand;
1209 if ((
id >= 0) && (
id < act_num_hand))
1210 return &act_hand.at(
id);
1220 return act_num_human;
1229 if ((
id >= 0) && (
id < act_num_human))
1230 return &act_human.at(
id);
1240 return act_num_inertial;
1249 if((
id >=0) && (
id < act_num_inertial))
1250 return &act_inertial.at(
id);
1260 return act_num_marker;
1269 if ((index >= 0) && (index < act_num_marker))
1270 return &act_marker.at(index);
1280 return act_framecounter;
1289 return act_timestamp;
1298 return act_timestamp_sec;
1307 return act_timestamp_usec;
1316 return act_latency_usec;
1325 return act_is_status_available;
1334 if ( ! act_is_status_available )
return NULL;
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).
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.