65 #define OS_UNIX // for Unix (Linux, Irix, ...)
67 #define OS_WIN // for MS Windows (2000, XP, ...)
82 #ifdef VRPN_USE_WINSOCK2
89 #include <netinet/in.h>
90 #include <sys/socket.h>
92 #include <sys/select.h>
102 #define DTRACK2VRPN_BUTTONS_PER_FLYSTICK 8 // number of vrpn buttons per Flystick (fixed)
103 #define DTRACK2VRPN_ANALOGS_PER_FLYSTICK 2 // number of vrpn analogs per Flystick (fixed)
105 #define UDPRECEIVE_BUFSIZE 20000 // size of udp buffer for DTrack data (one frame; in bytes)
111 #define DTRACK_ERR_NONE 0 // no error
112 #define DTRACK_ERR_TIMEOUT 1 // timeout while receiving data
113 #define DTRACK_ERR_UDP 2 // UDP receive error
114 #define DTRACK_ERR_PARSE 3 // error in UDP packet
118 static char* string_nextline(
char* str,
char* start,
int len);
119 static char* string_get_i(
char* str,
int* i);
120 static char* string_get_ui(
char* str,
unsigned int* ui);
121 static char* string_get_d(
char* str,
double* d);
122 static char* string_get_f(
char* str,
float* f);
123 static char* string_get_block(
char* str,
const char* fmt,
int* idat,
float* fdat);
141 int dtrackPort,
float timeToReachJoy,
142 int fixNbody,
int fixNflystick,
int* fixId,
143 bool act3DOFout,
bool actTracing) :
158 output_3dof_marker = act3DOFout;
159 tracing = actTracing;
162 tim_first.tv_sec = tim_first.tv_usec = 0;
163 tim_last.tv_sec = tim_last.tv_usec = 0;
167 if(fixNbody >= 0 && fixNflystick >= 0){
168 use_fix_numbering =
true;
170 fix_nbody = fixNbody;
171 fix_nflystick = fixNflystick;
173 fix_idbody.resize(fix_nbody + fix_nflystick);
174 fix_idflystick.resize(fix_nflystick);
177 for(i=0; i<fix_nbody + fix_nflystick; i++){
178 fix_idbody[i] = fixId[i];
181 for(i=0; i<fix_nflystick; i++){
182 fix_idflystick[i] = fixId[i + fix_nbody];
185 for(i=0; i<fix_nbody; i++){
186 for(
int j=0; j<fix_nflystick; j++){
187 if(fixId[i] < fixId[j + fix_nbody] && fix_idflystick[j] > 0){
193 for(i=0; i<fix_nbody + fix_nflystick; i++){
196 for(i=0; i<fix_nflystick; i++){
197 fix_idflystick[i] = i;
201 use_fix_numbering =
false;
204 warning_nbodycal =
false;
208 if(timeToReachJoy > 1e-20){
209 joy_incPerSec = 1.f / timeToReachJoy;
211 joy_incPerSec = 1e20f;
216 if(!dtrack_init(dtrackPort)){
242 int nbody, nflystick, i;
251 if(!dtrack_receive()){
253 fprintf(stderr,
"vrpn_Tracker_DTrack: Receive Error from DTrack.\n");
264 if(act_timestamp >= 0){
265 tts = (long )act_timestamp;
266 ttu = (long )((act_timestamp - tts) * 1000000);
269 if(tts >=
timestamp.tv_sec + 43200 - 1800){
271 }
else if(tts <=
timestamp.tv_sec - 43200 - 1800){
279 if(tim_first.tv_sec == 0 && tim_first.tv_usec == 0){
286 if(tracing && ((tracing_frames % 10) == 0)){
296 if(use_fix_numbering){
298 nflystick = fix_nflystick;
299 }
else if(act_has_bodycal_format){
300 nbody = act_num_bodycal;
301 nflystick = act_num_flystick;
303 if(!warning_nbodycal){
304 fprintf(stderr,
"vrpn_Tracker_DTrack warning: no DTrack '6dcal' data available.\n");
305 warning_nbodycal =
true;
308 nbody = act_num_body;
309 nflystick = act_num_flystick;
318 for(i=0; i<act_num_body; i++){
319 if(act_body[i].
id < nbody){
320 if(act_body[i].quality >= 0){
321 if(use_fix_numbering){
322 newid = fix_idbody[act_body[i].id];
324 newid = act_body[i].id;
327 dtrack2vrpn_body(newid,
"", act_body[i].
id, act_body[i].loc, act_body[i].rot,
timestamp);
332 if(
num_channel >= static_cast<int>(joy_last.size())){
333 size_t j0 = joy_last.size();
338 for(
size_t j=j0; j< static_cast<size_t>(
num_channel); j++){
339 joy_simulate[j] =
false;
344 for(i=0; i<(int)act_num_flystick; i++){
345 if(act_flystick[i].
id < nflystick){
346 if(act_flystick[i].quality >= 0){
347 if(use_fix_numbering){
348 newid = fix_idbody[act_flystick[i].id + nbody];
350 newid = act_flystick[i].id + nbody;
353 dtrack2vrpn_body(newid,
"f", act_flystick[i].
id, act_flystick[i].loc, act_flystick[i].rot,
357 if(use_fix_numbering){
358 newid = fix_idflystick[act_flystick[i].id];
360 newid = act_flystick[i].id;
363 dtrack2vrpn_flystickbuttons(newid, act_flystick[i].
id,
364 act_flystick[i].num_button, act_flystick[i].button,
timestamp);
366 dtrack2vrpn_flystickanalogs(newid, act_flystick[i].
id,
367 act_flystick[i].num_joystick, act_flystick[i].joystick, dt,
timestamp);
371 if (output_3dof_marker) {
374 for(i=0; i<act_num_marker; i++){
375 dtrack2vrpn_marker(offset + i,
"m", act_marker[i].
id, act_marker[i].loc,
timestamp);
403 int vrpn_Tracker_DTrack::dtrack2vrpn_marker(
int id,
const char* str_dtrack,
int id_dtrack,
404 const float* loc,
struct timeval timestamp)
411 pos[0] = loc[0] / 1000.;
412 pos[1] = loc[1] / 1000.;
413 pos[2] = loc[2] / 1000.;
417 q_make(
d_quat, 1, 0, 0, 0);
428 fprintf(stderr,
"vrpn_Tracker_DTrack: cannot write message: tossing.\n");
434 if(tracing && ((tracing_frames % 10) == 0)){
436 printf(
"marker id (DTrack vrpn): %s%d %d pos (x y z): %.4f %.4f %.4f\n",
437 str_dtrack, id_dtrack,
id,
pos[0],
pos[1],
pos[2]);
453 int vrpn_Tracker_DTrack::dtrack2vrpn_body(
int id,
const char* str_dtrack,
int id_dtrack,
454 const float* loc,
const float* rot,
struct timeval timestamp)
461 pos[0] = loc[0] / 1000.;
462 pos[1] = loc[1] / 1000.;
463 pos[2] = loc[2] / 1000.;
467 q_matrix_type destMatrix;
469 destMatrix[0][0] = rot[0];
470 destMatrix[0][1] = rot[1];
471 destMatrix[0][2] = rot[2];
472 destMatrix[0][3] = 0.0;
474 destMatrix[1][0] = rot[3];
475 destMatrix[1][1] = rot[4];
476 destMatrix[1][2] = rot[5];
477 destMatrix[1][3] = 0.0;
479 destMatrix[2][0] = rot[6];
480 destMatrix[2][1] = rot[7];
481 destMatrix[2][2] = rot[8];
482 destMatrix[2][3] = 0.0;
484 destMatrix[3][0] = 0.0;
485 destMatrix[3][1] = 0.0;
486 destMatrix[3][2] = 0.0;
487 destMatrix[3][3] = 1.0;
489 q_from_row_matrix(
d_quat, destMatrix);
500 fprintf(stderr,
"vrpn_Tracker_DTrack: cannot write message: tossing.\n");
506 if(tracing && ((tracing_frames % 10) == 0)){
507 q_vec_type yawPitchRoll;
509 q_to_euler(yawPitchRoll,
d_quat);
511 printf(
"body id (DTrack vrpn): %s%d %d pos (x y z): %.4f %.4f %.4f euler (y p r): %.3f %.3f %.3f\n",
512 str_dtrack, id_dtrack,
id,
pos[0],
pos[1],
pos[2],
513 Q_RAD_TO_DEG(yawPitchRoll[0]), Q_RAD_TO_DEG(yawPitchRoll[1]), Q_RAD_TO_DEG(yawPitchRoll[2]));
528 int vrpn_Tracker_DTrack::dtrack2vrpn_flystickbuttons(
int id,
int id_dtrack,
529 int num_but,
const int* but,
struct timeval timestamp)
550 if(act_has_old_flystick_format){
560 if(tracing && ((tracing_frames % 10) == 0)){
561 printf(
"flystick id (DTrack vrpn): f%d %d but ", id_dtrack,
id);
563 printf(
" %d", but[i]);
581 int vrpn_Tracker_DTrack::dtrack2vrpn_flystickanalogs(
int id,
int id_dtrack,
582 int num_ana,
const float* ana,
float dt,
struct timeval timestamp)
602 joy_simulate[ind] =
false;
603 }
else if((f > 0.99 || f < -0.99) && joy_last[ind] == 0){
604 joy_simulate[ind] =
true;
607 if(joy_simulate[ind]){
609 f = joy_last[ind] + joy_incPerSec * dt;
613 joy_simulate[ind] =
false;
616 f = joy_last[ind] - joy_incPerSec * dt;
620 joy_simulate[ind] =
false;
638 if(tracing && ((tracing_frames % 10) == 0)){
639 printf(
"flystick id (DTrack vrpn): f%d %d ana ", id_dtrack,
id);
641 printf(
" %.2f", ana[i]);
661 bool vrpn_Tracker_DTrack::dtrack_init(
int udpport)
669 if(udpport <= 0 || udpport > 65535){
670 fprintf(stderr,
"vrpn_Tracker_DTrack: Illegal UDP port %d.\n", udpport);
674 d_udpsock = udp_init((
unsigned short )udpport);
677 fprintf(stderr,
"vrpn_Tracker_DTrack: Cannot Initialize UDP Socket.\n");
687 d_udpbuf = (
char *)malloc(d_udpbufsize);
689 if(d_udpbuf == NULL){
692 fprintf(stderr,
"vrpn_Tracker_DTrack: Cannot Allocate Memory for UDP Buffer.\n");
698 act_framecounter = 0;
701 act_num_marker = act_num_body = act_num_flystick = 0;
702 act_has_bodycal_format =
false;
703 act_has_old_flystick_format =
false;
713 bool vrpn_Tracker_DTrack::dtrack_exit(
void)
718 if(d_udpbuf != NULL){
737 bool vrpn_Tracker_DTrack::dtrack_receive(
void)
740 int i, j, k, l, n, len, id;
744 int loc_num_bodycal, loc_num_flystick1, loc_num_meatool;
753 act_framecounter = 0;
756 loc_num_bodycal = -1;
757 loc_num_flystick1 = loc_num_meatool = 0;
759 act_has_bodycal_format =
false;
763 len = udp_receive(d_udpsock, d_udpbuf, d_udpbufsize-1, d_udptimeout_us);
784 if(!strncmp(s,
"fr ", 3)){
787 if(!(s = string_get_ui(s, &act_framecounter))){
788 act_framecounter = 0;
797 if(!strncmp(s,
"ts ", 3)){
800 if(!(s = string_get_d(s, &act_timestamp))){
810 if(!strncmp(s,
"6dcal ", 6)){
813 act_has_bodycal_format =
true;
815 if(!(s = string_get_i(s, &loc_num_bodycal))){
824 if(!strncmp(s,
"3d ", 3)){
828 if(!(s = string_get_i(s, &n))){
832 if (static_cast<unsigned>(n) > act_marker.size()) {
833 act_marker.resize(n);
837 if(!(s = string_get_block(s,
"if", &
id, &f))){
841 act_marker[act_num_marker].id = id;
843 if(!(s = string_get_block(s,
"fff", NULL, act_marker[act_num_marker].loc))){
855 if(!strncmp(s,
"6d ", 3)){
858 for(i=0; i<act_num_body; i++){
861 act_body[i].quality = -1;
864 if(!(s = string_get_i(s, &n))){
869 if(!(s = string_get_block(s,
"if", &
id, &f))){
873 if(
id >= act_num_body){
874 act_body.resize(
id + 1);
876 for(j=act_num_body; j<=id; j++){
879 act_body[j].quality = -1;
882 act_num_body =
id + 1;
885 act_body[id].id = id;
886 act_body[id].quality = f;
888 if(!(s = string_get_block(s,
"fff", NULL, act_body[
id].loc))){
892 if(!(s = string_get_block(s,
"fffffffff", NULL, act_body[
id].rot))){
902 if(!strncmp(s,
"6df ", 4)){
905 act_has_old_flystick_format =
true;
907 if(!(s = string_get_i(s, &n))){
911 loc_num_flystick1 = n;
913 if(n > act_num_flystick){
914 act_flystick.resize(n);
916 act_num_flystick = n;
920 if(!(s = string_get_block(s,
"ifi", iarr, &f))){
928 act_flystick[i].id = iarr[0];
929 act_flystick[i].quality = f;
931 act_flystick[i].num_button = 8;
934 act_flystick[i].button[j] = k & 0x01;
938 act_flystick[i].num_joystick = 2;
940 act_flystick[i].joystick[0] = -1;
941 }
else if(iarr[1] & 0x80){
942 act_flystick[i].joystick[0] = 1;
944 act_flystick[i].joystick[0] = 0;
947 act_flystick[i].joystick[1] = -1;
948 }
else if(iarr[1] & 0x40){
949 act_flystick[i].joystick[1] = 1;
951 act_flystick[i].joystick[1] = 0;
954 if(!(s = string_get_block(s,
"fff", NULL, act_flystick[i].loc))){
958 if(!(s = string_get_block(s,
"fffffffff", NULL, act_flystick[i].rot))){
968 if(!strncmp(s,
"6df2 ", 5)){
971 act_has_old_flystick_format =
false;
973 if(!(s = string_get_i(s, &n))){
977 if(n > act_num_flystick){
978 act_flystick.resize(n);
980 act_num_flystick = n;
983 if(!(s = string_get_i(s, &n))){
988 if(!(s = string_get_block(s,
"ifii", iarr, &f))){
996 act_flystick[i].id = iarr[0];
997 act_flystick[i].quality = f;
1005 act_flystick[i].num_button = iarr[1];
1006 act_flystick[i].num_joystick = iarr[2];
1008 if(!(s = string_get_block(s,
"fff", NULL, act_flystick[i].loc))){
1012 if(!(s = string_get_block(s,
"fffffffff", NULL, act_flystick[i].rot))){
1018 while(j < act_flystick[i].num_button){
1023 while(j < act_flystick[i].num_joystick){
1028 if(!(s = string_get_block(s, sfmt, iarr, act_flystick[i].joystick))){
1033 for(j=0; j<act_flystick[i].num_button; j++){
1034 act_flystick[i].button[j] = iarr[k] & 0x01;
1050 if(!strncmp(s,
"6dmt ", 5)){
1053 if(!(s = string_get_i(s, &n))){
1057 loc_num_meatool = n;
1064 }
while((s = string_nextline(d_udpbuf, s, d_udpbufsize)) != NULL);
1068 if(loc_num_bodycal >= 0){
1069 act_num_bodycal = loc_num_bodycal - loc_num_flystick1 - loc_num_meatool;
1087 static char* string_nextline(
char* str,
char* start,
int len)
1090 char* se = str + len;
1094 if(*s ==
'\r' || *s ==
'\n'){
1098 return (*s) ? s : NULL;
1114 static char* string_get_i(
char* str,
int* i)
1118 *i = (int )strtol(str, &s, 0);
1119 return (s == str) ? NULL : s;
1128 static char* string_get_ui(
char* str,
unsigned int* ui)
1132 *ui = (
unsigned int )strtoul(str, &s, 0);
1133 return (s == str) ? NULL : s;
1142 static char* string_get_d(
char* str,
double* d)
1146 *d = strtod(str, &s);
1147 return (s == str) ? NULL : s;
1156 static char* string_get_f(
char* str,
float* f)
1160 *f = (float )strtod(str, &s);
1161 return (s == str) ? NULL : s;
1172 static char* string_get_block(
char* str,
const char* fmt,
int* idat,
float* fdat)
1175 int index_i, index_f;
1177 if((str = strchr(str,
'[')) == NULL){
1180 if((strend = strchr(str,
']')) == NULL){
1187 index_i = index_f = 0;
1192 if((str = string_get_i(str, &idat[index_i++])) == NULL){
1199 if((str = string_get_f(str, &fdat[index_f++])) == NULL){
1223 #define INVALID_SOCKET -1
1233 struct sockaddr_in name;
1242 vreq = MAKEWORD(2, 0);
1244 if(WSAStartup(vreq, &wsa) != 0){
1256 usock = socket(PF_INET, SOCK_DGRAM, 0);
1268 wsock = socket(PF_INET, SOCK_DGRAM, 0);
1281 name.sin_family = AF_INET;
1282 name.sin_port = htons(port);
1283 name.sin_addr.s_addr = htonl(INADDR_ANY);
1285 if(bind(sock, (
struct sockaddr *) &name,
sizeof(name)) < 0){
1306 err = closesocket(sock);
1328 #pragma warning ( disable : 4127 )
1335 struct timeval tout;
1342 tout.tv_sec = tout_us / 1000000;
1343 tout.tv_usec = tout_us % 1000000;
1345 switch((err = select(FD_SETSIZE, &set, NULL, NULL, &tout))){
1360 nbytes = recv(sock, (
char *)buffer, maxlen, 0);
1374 if(select(FD_SETSIZE, &set, NULL, NULL, &tout) != 1){
1378 if(nbytes >= maxlen){