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
103#define DTRACK2VRPN_ANALOGS_PER_FLYSTICK 2
105#define UDPRECEIVE_BUFSIZE 20000
111#define DTRACK_ERR_NONE 0
112#define DTRACK_ERR_TIMEOUT 1
113#define DTRACK_ERR_UDP 2
114#define DTRACK_ERR_PARSE 3
118static char* string_nextline(
char* str,
char* start,
int len);
119static char* string_get_i(
char* str,
int* i);
120static char* string_get_ui(
char* str,
unsigned int* ui);
121static char* string_get_d(
char* str,
double* d);
122static char* string_get_f(
char* str,
float* f);
123static 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);
403int 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]);
453int 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]));
528int 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]);
581int 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]);
661bool 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;
713bool vrpn_Tracker_DTrack::dtrack_exit(
void)
718 if(d_udpbuf != NULL){
737bool 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;
1087static 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;
1114static char* string_get_i(
char* str,
int* i)
1118 *i = (int )strtol(str, &s, 0);
1119 return (s == str) ? NULL : s;
1128static char* string_get_ui(
char* str,
unsigned int* ui)
1132 *ui = (
unsigned int )strtoul(str, &s, 0);
1133 return (s == str) ? NULL : s;
1142static char* string_get_d(
char* str,
double* d)
1146 *d = strtod(str, &s);
1147 return (s == str) ? NULL : s;
1156static char* string_get_f(
char* str,
float* f)
1160 *f = (float )strtod(str, &s);
1161 return (s == str) ? NULL : s;
1172static 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){
vrpn_float64 channel[vrpn_CHANNEL_MAX]
virtual void report_changes(vrpn_uint32 class_of_service=vrpn_CONNECTION_LOW_LATENCY, const struct timeval time=vrpn_ANALOG_NOW)
Send a report only if something has changed (for servers) Optionally, tell what time to stamp the val...
vrpn_Connection * d_connection
Connection that this object talks to.
vrpn_int32 d_sender_id
Sender ID registered with the connection.
void server_mainloop(void)
Handles functions that all servers should provide in their mainloop() (ping/pong, for example) Should...
Generic connection class not specific to the transport mechanism.
virtual int pack_message(vrpn_uint32 len, struct timeval time, vrpn_int32 type, vrpn_int32 sender, const char *buffer, vrpn_uint32 class_of_service)
Pack a message that will be sent the next time mainloop() is called. Turn off the RELIABLE flag if yo...
virtual void mainloop()
This function should be called each time through the main loop of the server code....
vrpn_Tracker_DTrack(const char *name, vrpn_Connection *c, int dtrackPort, float timeToReachJoy=0.f, int fixNbody=-1, int fixNflystick=-1, int *fixId=NULL, bool act3DOFout=false, bool actTracing=false)
virtual int encode_to(char *buf)
const vrpn_uint32 vrpn_CONNECTION_LOW_LATENCY
Header containing macros formerly duplicated in a lot of implementation files.
double vrpn_TimevalDurationSeconds(struct timeval endT, struct timeval startT)
Return the number of seconds between startT and endT as a floating-point value.
#define vrpn_gettimeofday
#define DTRACK2VRPN_ANALOGS_PER_FLYSTICK
#define DTRACK_ERR_TIMEOUT
#define DTRACK2VRPN_BUTTONS_PER_FLYSTICK
#define UDPRECEIVE_BUFSIZE
#define vrpn_DTRACK_FLYSTICK_MAX_BUTTON
#define vrpn_DTRACK_FLYSTICK_MAX_JOYSTICK