1 /*
2 * InputDevice.cpp
3 * 2001-2008 J. "MUFTI" Scheurich
4 */
5
6 /*
7 * This program is free software; you can redistribute it and/or modify
8 * it under the terms of the GNU General Public License as published by
9 * the Free Software Foundation; either version 2 of the License, or
10 * (at your option) any later version.
11 *
12 * This program is distributed in the hope that it will be useful,
13 * but WITHOUT ANY WARRANTY; without even the implied warranty of
14 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15 * GNU General Public License for more details.
16 *
17 * You should have received a copy of the GNU General Public License
18 * along with this program; if not, write to the Free Software
19 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
20 */
21
22 #include "InputDevice.h"
23 #include <string.h>
24 #include <math.h>
25 #include "SFRotation.h"
26
27 #include <sys/types.h>
28 #include <sys/stat.h>
29 #ifndef _WIN32
30 # include <unistd.h>
31 #endif
32
33 #ifdef _WIN32
34 #if _MSC_VER < 1800
copysign(double x,double y)35 double copysign (double x, double y)
36 {
37 if (y>=0)
38 return fabs(x);
39 else
40 return -fabs(x);
41 }
42 #endif
43 #endif
44
accelerate(float value,int num_axis)45 float InputDevice::accelerate(float value,int num_axis)
46 {
47 float result=0;
48 if (get_acceleration(num_axis)!=1.0)
49 {
50 # ifdef HAVE_POWF
51 result=copysign(powf(get_acceleration(num_axis),fabs(value))-1,value);
52 # else
53 result=copysign(pow((double)get_acceleration(num_axis),fabs(value))-1,
54 value);
55 # endif
56 }
57 result+=value*get_factor(num_axis);
58 return result*get_sign(num_axis);
59 }
60
max_position(float f1,float f2,float f3)61 static int max_position(float f1,float f2,float f3)
62 {
63 if (fabs(f1) >= fabs(f2))
64 if (fabs(f1) >= fabs(f3))
65 return 0;
66 if (fabs(f2) >= fabs(f3))
67 if (fabs(f2) >= fabs(f1))
68 return 1;
69 if (fabs(f3) >= fabs(f2))
70 if (fabs(f3) >= fabs(f1))
71 return 2;
72 assert(0);
73 return -1;
74 }
75
min_position(float f1,float f2,float f3)76 static int min_position(float f1,float f2,float f3)
77 {
78 if (fabs(f1) <= fabs(f2) )
79 if (fabs(f1) <= fabs(f3))
80 return 0;
81 if (fabs(f2) <= fabs(f3))
82 if (fabs(f2) <= fabs(f1))
83 return 1;
84 if (fabs(f3) <= fabs(f2))
85 if (fabs(f3) <= fabs(f1))
86 return 2;
87 assert(0);
88 return -1;
89 }
90
91 // test if value belongs to the greater values in 1, 2 (or 3) dimensions
92
max_value(int index,TransformMode * tm)93 bool InputDevice::max_value(int index,TransformMode* tm)
94 {
95 if (tm==NULL)
96 return true;
97 if (tm->tdimension==TM_3D)
98 return(true);
99 if (tm->tdimension==TM_2D) {
100 if (index<3)
101 {
102 if (index!=min_position(value[0],value[1],value[2]))
103 return true;
104 }
105 else
106 if (index!=(3+min_position(value[3],value[4],value[5])))
107 return true;
108 }
109 if (tm->tdimension==TM_1D) {
110 if (index<3)
111 {
112 if (index==max_position(value[0],value[1],value[2]))
113 return true;
114 }
115 else
116 if (index==(3+max_position(value[3],value[4],value[5])))
117 return true;
118 }
119 return false;
120 }
121
set_firstflag(void)122 void InputDevice::set_firstflag(void)
123 {
124 for (int i=0;i<7;i++)
125 if (!get_zero_on_release(i))
126 firstflag[i]=true;
127 }
128
129
130 // return value, if device deliver zero if you release it,
131 // otherwise return difference to last value
132
get_value(int index,bool check_only,int num_axis)133 float InputDevice::get_value(int index,bool check_only,int num_axis)
134 {
135 // value > zerosize ?
136 float zerosize=get_zero_size_fraction(num_axis)*maxvalue(num_axis);
137 if (fabs(value[index]) < zerosize)
138 return 0;
139 float val;
140 // subtract nullsize from value
141 if (value[index]>0)
142 val=value[index]-zerosize;
143 else
144 val=value[index]+zerosize;
145 if (get_zero_on_release(num_axis))
146 return(accelerate(val,num_axis));
147 else
148 {
149 float ret;
150 if (firstflag[index])
151 {
152 ret=0.0;
153 if (check_only)
154 {
155 if (val!=0)
156 {
157 firstflag[index]=false;
158 oldvalue[index]=val;
159 }
160 return(0);
161 }
162 firstflag[index]=false;
163 }
164 else
165 ret=accelerate(val,num_axis)-
166 accelerate(oldvalue[index],num_axis);
167 if (!check_only)
168 oldvalue[index]=val;
169 return ret;
170 }
171 }
172
get_value(TransformMode * tm,int index,bool check_only,int num_axis,bool current)173 float InputDevice::get_value(TransformMode* tm, int index, bool check_only,
174 int num_axis, bool current)
175 {
176 if (ignore[num_axis])
177 return 0;
178 if (current)
179 {
180 // value > zerosize ?
181 float zerosize=get_zero_size_fraction(num_axis)*maxvalue(num_axis);
182 if (fabs(value[index]) < zerosize)
183 return 0;
184 float val;
185 // subtract nullsize from value
186 if (value[index]>0)
187 val=value[index]-zerosize;
188 else
189 val=value[index]+zerosize;
190 return val;
191 }
192 else
193 {
194 if (!max_value(index,tm))
195 return 0.0;
196 return get_value(index,check_only,num_axis);
197 }
198 }
199
200 #define INPUT_DEVICE_GET(functionname,index,num_axis) \
201 float functionname (TransformMode* tm,bool check_only, bool current) \
202 { return get_value(tm, index, check_only, num_axis, current); }
203
204 INPUT_DEVICE_GET(InputDevice::get_x,0,num_axis_x)
205 INPUT_DEVICE_GET(InputDevice::get_y,1,num_axis_y)
206 INPUT_DEVICE_GET(InputDevice::get_z,2,num_axis_z)
207 INPUT_DEVICE_GET(InputDevice::get_xrot,3,num_axis_xrot)
208 INPUT_DEVICE_GET(InputDevice::get_yrot,4,num_axis_yrot)
209 INPUT_DEVICE_GET(InputDevice::get_zrot,5,num_axis_zrot)
210 INPUT_DEVICE_GET(InputDevice::get_angle,6,num_axis_angle)
211
get_vector(TransformMode * tm)212 Vec3f& InputDevice::get_vector(TransformMode* tm)
213 {
214 static Vec3f v;
215 v.x=get_x(tm);
216 v.y=get_y(tm);
217 v.z=get_z(tm);
218 if (get_number_axes()==4)
219 v[2]=-v[2];
220 TMode mode=tm->tmode;
221 if (mode==TM_TRANSLATE)
222 {
223 if (get_number_axes()==2)
224 if (tm->t2axes==TM_NEAR_FAR)
225 {
226 float temp=v[2];
227 v[2]=-v[1];
228 v[1]=temp;
229 }
230 }
231 else if (mode==TM_ROCKET)
232 {
233 v[0]=0;
234 v[1]=0;
235 }
236 else if (mode==TM_HOVER)
237 {
238 if (get_number_axes()==3)
239 {
240 v[0]=0;
241 }
242 else if (get_number_axes()==2)
243 {
244 if (tm->t2axes==TM_NEAR_FAR)
245 {
246 v[2]=-v[1];
247 v[0]=0;
248 v[1]=0;
249 }
250 }
251 else if (tm->t2axes==TM_UP_DOWN)
252 {
253 v[1]=v[1];
254 v[0]=0;
255 v[2]=0;
256 }
257 }
258 else if (mode==TM_SCALE)
259 {
260 v[0]=-v[0];
261 v[2]=-v[2];
262 if (get_number_axes()==2)
263 if (tm->t2axes==TM_NEAR_FAR)
264 {
265 float temp=v[2];
266 v[2]=-v[1];
267 v[1]=temp;
268 }
269 }
270 else if (mode==TM_UNIFORM_SCALE)
271 {
272 v[0]=-v[0];
273 v[2]=-v[2];
274 if (get_number_axes()==2)
275 if (tm->t2axes==TM_NEAR_FAR)
276 {
277 float temp=v[2];
278 v[2]=-v[1];
279 v[1]=temp;
280 }
281 int max_index = max_position(v[0],v[1],v[2]);
282 if (max_index == 0)
283 {
284 v[1] = 0.0f;
285 v[2] = 0.0f;
286 }
287 else if (max_index == 1)
288 {
289 v[0] = 0.0f;
290 v[2] = 0.0f;
291 }
292 else
293 {
294 v[0] = 0.0f;
295 v[1] = 0.0f;
296 }
297 }
298 else if (mode==TM_CENTER)
299 {
300 if (get_number_axes()==2)
301 if (tm->t2axes==TM_NEAR_FAR)
302 {
303 float temp=v[2];
304 v[2]=-v[1];
305 v[1]=temp;
306 }
307 }
308 for (int i=0;i<3;i++)
309 v[i] *= xyzfactor;
310 return v;
311 }
312
get_eulerAngles(TransformMode * tm,bool check_only,bool current)313 EulerAngles& InputDevice::get_eulerAngles(TransformMode* tm, bool check_only,
314 bool current)
315 {
316 bool rotationOnly=false;
317 bool rocketOnly=false;
318 bool hoverOnly=false;
319 static EulerAngles euler;
320 // check for constraints on 2 axes or 3 axes or 4 axes devices
321 if (get_number_axes()<=4)
322 if (tm!=NULL) {
323 if (tm->tmode==TM_ROTATE)
324 rotationOnly=true;
325 else if (tm->tmode==TM_ROCKET)
326 rocketOnly=true;
327 }
328 if (tm!=NULL)
329 if (tm->tmode==TM_HOVER)
330 hoverOnly=true;
331 if (rotationOnly)
332 {
333 if (get_number_axes()==4)
334 {
335 euler.x=/*RAD2DEG*/(-get_y(tm, check_only, current));
336 euler.y=/*RAD2DEG*/( get_xrot(tm, check_only, current));
337 euler.z=/*RAD2DEG*/(-get_x(tm, check_only, current));
338 }
339 else if (get_number_axes()==3)
340 {
341 euler.x=/*RAD2DEG*/(-get_y(tm, check_only, current));
342 euler.y=/*RAD2DEG*/( get_z(tm, check_only, current));
343 euler.z=/*RAD2DEG*/(-get_x(tm, check_only, current));
344 }
345 else if (get_number_axes()==2)
346 {
347 if (tm->t2axes==TM_NEAR_FAR)
348 {
349 euler.x=/*RAD2DEG*/(-get_y(tm, check_only, current));
350 euler.y=/*RAD2DEG*/(get_x(tm, check_only, current));
351 euler.z=0;
352 }
353 else
354 {
355 euler.x=/*RAD2DEG*/(-get_y(tm, check_only, current));
356 euler.y=0;
357 euler.z=/*RAD2DEG*/(-get_x(tm, check_only, current));
358 }
359 }
360 }
361 else if (rocketOnly)
362 {
363 if (get_number_axes()==4)
364 {
365 euler.x=/*RAD2DEG*/(-get_y(tm, check_only, current));
366 euler.y=/*RAD2DEG*/(-get_xrot(tm, check_only, current));
367 euler.z=/*RAD2DEG*/(-get_x(tm, check_only, current));
368 }
369 else if (get_number_axes()==3)
370 {
371 if (tm->t2axes==TM_NEAR_FAR)
372 {
373 euler.x=/*RAD2DEG*/(-get_y(tm, check_only, current));
374 euler.y=/*RAD2DEG*/(-get_x(tm, check_only, current));
375 euler.z=0;
376 }
377 else
378 {
379 euler.x=/*RAD2DEG*/(-get_y(tm, check_only, current));
380 euler.y=0;
381 euler.z=/*RAD2DEG*/(-get_x(tm, check_only, current));
382 }
383 }
384 }
385 else if (hoverOnly)
386 {
387 if (get_number_axes()==4)
388 {
389 euler.x=0;
390 euler.y=/*RAD2DEG*/(-get_xrot(tm, check_only, current));
391 euler.z=0;
392 }
393 else if (get_number_axes()==3)
394 {
395 euler.x=0;
396 euler.y=/*RAD2DEG*/(-get_x(tm, check_only, current));
397 euler.z=0;
398 }
399 else if (get_number_axes()==2)
400 {
401 euler.x=0;
402 euler.y=/*RAD2DEG*/(-get_x(tm, check_only, current));
403 euler.z=0;
404 }
405 else
406 {
407 euler.x=0;
408 euler.y=/*RAD2DEG*/( get_yrot(tm, check_only, current));
409 euler.z=0;
410 }
411 }
412 else
413 {
414 euler.x=/*RAD2DEG*/( get_xrot(tm, check_only, current));
415 euler.y=/*RAD2DEG*/( get_yrot(tm, check_only, current));
416 euler.z=/*RAD2DEG*/(-get_zrot(tm, check_only, current));
417 }
418
419 if (!current)
420 {
421 euler.x *= rotfactor;
422 euler.y *= rotfactor;
423 euler.z *= rotfactor;
424 }
425 euler.w=EulOrdXYZs;
426 return euler;
427 }
428
get_quaternion(TransformMode * tm,bool local,bool check_only)429 Quaternion& InputDevice::get_quaternion(TransformMode* tm,
430 bool local, bool check_only)
431 {
432 Quaternion inputrot;
433 EulerAngles euler=get_eulerAngles(tm, check_only, useCurrent());
434 if (local)
435 {
436 euler.x=-euler.x;
437 euler.z=-euler.z;
438 }
439 inputrot=Eul_ToQuat(euler);
440 inputrot.normalize();
441 static Quaternion ret;
442 if (useCurrent())
443 {
444 SFRotation rot(inputrot);
445 const float *quat=rot.getValue();
446 float arc=quat[3]*rotfactor;
447 SFRotation rot2(quat[0],quat[1],quat[2],arc);
448 inputrot=rot2.getQuat();
449 inputrot.normalize();
450 if (get_zero_on_release(num_axis_angle))
451 ret=inputrot;
452 else
453 {
454 if (firstflag[num_axis_angle]==true)
455 firstflag[num_axis_angle]=false;
456 else
457 if (local)
458 ret=old_quat.conj()*inputrot;
459 else
460 ret=inputrot*old_quat.conj();
461 if (!check_only)
462 old_quat=inputrot;
463 }
464 SFRotation rot3(ret);
465 const float *v=rot3.getValue();
466 float value[4] = { v[0], v[1], v[2], v[3] };
467 // limitation of axis make no sense for "wheel" type devices
468 if (!isTracker() && get_zero_on_release(num_axis_angle))
469 {
470 if (tm->tdimension==TM_2D)
471 {
472 int index=min_position(value[0],value[1],value[2]);
473 value[index]=0;
474 }
475 else if (tm->tdimension==TM_1D)
476 {
477 int index=max_position(value[0],value[1],value[2]);
478 for (int i=0;i<3;i++)
479 if (i!=index)
480 value[i]=0;
481 }
482 }
483 SFRotation rot4(-value[0],value[1],value[2],value[3]);
484 ret=rot4.getQuat();
485 ret.normalize();
486 }
487 else
488 ret=inputrot;
489 return ret;
490 }
491
get_quaternion(TransformMode * tm,bool check_only)492 Quaternion& InputDevice::get_quaternion(TransformMode* tm, bool check_only)
493 {
494 return get_quaternion(tm, false, check_only);
495 }
496
get_localQuaternion(TransformMode * tm,bool check_only)497 Quaternion& InputDevice::get_localQuaternion(TransformMode* tm,
498 bool check_only)
499 {
500 return get_quaternion(tm, true, check_only);
501 }
502
allzero(void)503 bool InputDevice::allzero(void)
504 {
505 if (useCurrent())
506 return false;
507 if ((get_x(NULL,true)==0) && (get_y(NULL,true)==0) && (get_z(NULL,true)==0)
508 && (get_xrot(NULL,true)==0)
509 && (get_yrot(NULL,true)==0)
510 && (get_zrot(NULL,true)==0)
511 )
512 return true;
513 else
514 return false;
515 }
516
517 // signswap of value from axis
518
get_sign(int axis_number)519 int InputDevice::get_sign(int axis_number)
520 {
521 if ((sign_swap==NULL) || (axis_number>=number_max_axis))
522 return 1;
523 else
524 if (sign_swap[axis_number])
525 return -1;
526 else
527 return 1;
528 }
529
set_sign_swap(int axis_number,bool value)530 void InputDevice::set_sign_swap(int axis_number,bool value)
531 {
532 if (axis_number<number_max_axis)
533 {
534 if (sign_swap==NULL)
535 {
536 sign_swap=new bool[number_max_axis];
537 for (int i=0;i<number_max_axis;i++)
538 sign_swap[i]=false;
539 }
540 sign_swap[axis_number]=value;
541 }
542 else
543 swDebugf("axis number %d do not exist \n",axis_number);
544 }
545
546 // factor: multiplied to the axis value returned from device
547 // deliver 1.0 if factor is not set
548
get_factor(int axis_number)549 float InputDevice::get_factor(int axis_number)
550 {
551 if ((factor==NULL) || (axis_number>=number_max_axis))
552 return 1.0;
553 else
554 return factor[axis_number];
555 }
556
set_factor(int axis_number,float value)557 void InputDevice::set_factor(int axis_number,float value)
558 {
559 if (axis_number<number_max_axis)
560 {
561 if (factor==NULL)
562 {
563 factor=new float[number_max_axis];
564 for (int i=0;i<number_max_axis;i++)
565 factor[i]=1.0;
566 }
567 factor[axis_number]=value;
568 }
569 else
570 swDebugf("axis number %d do not exist \n",axis_number);
571 }
572
set_factor(const char * string,float value)573 void InputDevice::set_factor(const char* string,float value)
574 {
575 for (unsigned int i=0;i<sizeof(axesinfo)/sizeof(char*);i++)
576 if (stringncmp(string,axesinfo[i])==0)
577 {
578 set_factor(getAxisFromInformation(string),value);
579 break;
580 }
581 if (stringncmp(string,"all")==0)
582 {
583 if (stringncmp(string,"allrot")!=0)
584 xyzfactor=value;
585 if (stringncmp(string,"allxyz")!=0)
586 rotfactor=value;
587 }
588 }
589
590 // acceleration: additional acceleration for devices without acceleration
591 // deliver 1.0 (no acceleration) if acceleration is not set
592
get_acceleration(int axis_number)593 float InputDevice::get_acceleration(int axis_number)
594 {
595 if ((acceleration==NULL) || (axis_number>=number_max_axis))
596 return 1.0;
597 else
598 return acceleration[axis_number];
599 }
600
set_acceleration(int axis_number,float value)601 void InputDevice::set_acceleration(int axis_number,float value)
602 {
603 if (axis_number<number_max_axis)
604 {
605 if (acceleration==NULL)
606 {
607 acceleration=new float[number_max_axis];
608 for (int i=0;i<number_max_axis;i++)
609 acceleration[i]=1.0;
610 }
611 acceleration[axis_number]=value;
612 }
613 else
614 swDebugf("axis number %d do not exist \n",axis_number);
615 }
616
set_acceleration(const char * string,float value)617 void InputDevice::set_acceleration(const char* string,float value)
618 {
619 for (unsigned int i=0;i<sizeof(axesinfo)/sizeof(char*);i++)
620 if (stringncmp(string,axesinfo[i])==0)
621 {
622 set_acceleration(getAxisFromInformation(string),value);
623 break;
624 }
625 if (stringncmp(string,"all")==0)
626 {
627 if (stringncmp(string,"allrot")!=0)
628 {
629 set_acceleration(num_axis_x,value);
630 set_acceleration(num_axis_y,value);
631 set_acceleration(num_axis_z,value);
632 }
633 if (stringncmp(string,"allxyz")!=0)
634 {
635 set_acceleration(num_axis_xrot,value);
636 set_acceleration(num_axis_yrot,value);
637 set_acceleration(num_axis_zrot,value);
638 }
639 }
640 }
641
642 // return true if axis number deliver zero when device is released
643
get_zero_on_release(int axis_number)644 bool InputDevice::get_zero_on_release(int axis_number)
645 {
646 if ((zero_on_release==NULL) || (axis_number>=number_max_axes))
647 return false;
648 else
649 return zero_on_release[axis_number];
650 }
651
set_zero_on_release(int axis_number,bool value)652 void InputDevice::set_zero_on_release(int axis_number,bool value)
653 {
654 if (axis_number<number_max_axes)
655 {
656 if (zero_on_release==NULL)
657 {
658 zero_on_release=new bool[number_max_axes];
659 for (int i=0;i<number_max_axes;i++)
660 zero_on_release[i]=true;
661 }
662 zero_on_release[axis_number]=value;
663 }
664 else
665 swDebugf("axis number %d do not exist \n",axis_number);
666 }
667
set_zero_on_release(const char * string,bool value)668 void InputDevice::set_zero_on_release(const char* string,bool value)
669 {
670 for (unsigned int i=0;i<sizeof(axesinfo)/sizeof(char*);i++)
671 if (stringncmp(string,axesinfo[i])==0)
672 {
673 set_zero_on_release(getAxisFromInformation(string),value);
674 break;
675 }
676 if (stringncmp(string,"all")==0)
677 {
678 if (stringncmp(string,"allrot")!=0)
679 {
680 set_zero_on_release(num_axis_x,value);
681 set_zero_on_release(num_axis_y,value);
682 set_zero_on_release(num_axis_z,value);
683 }
684 if (stringncmp(string,"allxyz")!=0)
685 {
686 set_zero_on_release(num_axis_xrot,value);
687 set_zero_on_release(num_axis_yrot,value);
688 set_zero_on_release(num_axis_zrot,value);
689 set_zero_on_release(num_axis_angle,value);
690 }
691 }
692 }
693
694
695 // return value (relative to max) which will be ignored
696
get_zero_size_fraction(int axis_number)697 float InputDevice::get_zero_size_fraction(int axis_number)
698 {
699 if ((zero_size_fraction==NULL) || (axis_number>=number_max_axis))
700 return 0;
701 else
702 return zero_size_fraction[axis_number];
703 }
704
set_zero_size_fraction(int axis_number,float value)705 void InputDevice::set_zero_size_fraction(int axis_number,float value)
706 {
707 if (axis_number<number_max_axis)
708 {
709 if (zero_size_fraction==NULL)
710 {
711 zero_size_fraction=new float[number_max_axis];
712 for (int i=0;i<number_max_axis;i++)
713 zero_size_fraction[i]=0;
714 }
715 zero_size_fraction[axis_number]=value;
716 }
717 else
718 swDebugf("axis number %d do not exist \n",axis_number);
719 }
720
set_zero_size_fraction(const char * string,float value)721 void InputDevice::set_zero_size_fraction(const char* string,float value)
722 {
723 for (unsigned int i=0;i<sizeof(axesinfo)/sizeof(char*);i++)
724 if (stringncmp(string,axesinfo[i])==0)
725 {
726 set_zero_size_fraction(getAxisFromInformation(string),value);
727 break;
728 }
729 if (stringncmp(string,"all")==0)
730 {
731 if (stringncmp(string,"allrot")!=0)
732 {
733 if (num_axis_x < get_number_max_axis())
734 set_zero_size_fraction(num_axis_x,value);
735 if (num_axis_y < get_number_max_axis())
736 set_zero_size_fraction(num_axis_y,value);
737 if (num_axis_z < get_number_max_axis())
738 set_zero_size_fraction(num_axis_z,value);
739 }
740 if (stringncmp(string,"allxyz")!=0)
741 {
742 if (num_axis_xrot < get_number_max_axis())
743 set_zero_size_fraction(num_axis_xrot,value);
744 if (num_axis_yrot < get_number_max_axis())
745 set_zero_size_fraction(num_axis_yrot,value);
746 if (num_axis_zrot < get_number_max_axis())
747 set_zero_size_fraction(num_axis_zrot,value);
748 }
749 }
750 }
751
getAxisFromInformation(const char * axesinfo)752 int InputDevice::getAxisFromInformation(const char* axesinfo)
753 {
754 int axis_number;
755
756 if (strcmp(axesinfo,"xrot")==0)
757 axis_number=num_axis_xrot;
758 else if (strcmp(axesinfo,"yrot")==0)
759 axis_number=num_axis_yrot;
760 else if (strcmp(axesinfo,"zrot")==0)
761 axis_number=num_axis_zrot;
762 else if (strcmp(axesinfo,"x")==0)
763 axis_number=num_axis_x;
764 else if (strcmp(axesinfo,"y")==0)
765 axis_number=num_axis_y;
766 else if (strcmp(axesinfo,"z")==0)
767 axis_number=num_axis_z;
768 else
769 {
770 swDebugf("axesinfo %s not understood, using axis 0\n",axesinfo);
771 axis_number=0;
772 }
773 return axis_number;
774 }
775
setAxisOfInformation(const char * axesinfo,int axis_number)776 void InputDevice::setAxisOfInformation(const char* axesinfo,int axis_number)
777 {
778 if (strcmp(axesinfo,"xrot")==0)
779 num_axis_xrot=axis_number;
780 else if (strcmp(axesinfo,"yrot")==0)
781 num_axis_yrot=axis_number;
782 else if (strcmp(axesinfo,"zrot")==0)
783 num_axis_zrot=axis_number;
784 else if (strcmp(axesinfo,"x")==0)
785 num_axis_x=axis_number;
786 else if (strcmp(axesinfo,"y")==0)
787 num_axis_y=axis_number;
788 else if (strcmp(axesinfo,"z")==0)
789 num_axis_z=axis_number;
790 else if (strcmp(axesinfo,"none")==0)
791 {
792 ignore[axis_number]=true;
793 number_ignored_axes++;
794 }
795 }
796
797 // destruktive way to cut out the string till next comma
798 // return index to the comma position or -1 if no comma position available
799
cut_out_till_next_comma(char * string)800 static int cut_out_till_next_comma(char* string)
801 {
802 char* comma;
803 // search for comma
804 comma=strchr(string,',');
805 if (comma==NULL)
806 return -1;
807 else
808 {
809 // end of string is placed at comma position
810 comma[0]=(char)0;
811 return(comma-string+1);
812 }
813 }
814
string2int(int & value,const char * string)815 static bool string2int(int &value,const char* string)
816 {
817 unsigned int characters_read;
818
819 sscanf(string,"%d%n",&value,&characters_read);
820 if (characters_read!=strlen(string))
821 return(false);
822 return(true);
823 }
824
string2float(float & value,const char * string)825 static bool string2float(float &value,const char* string)
826 {
827 unsigned int characters_read;
828
829 sscanf(string,"%f%n",&value,&characters_read);
830 if (characters_read!=strlen(string))
831 return(false);
832 return(true);
833 }
834
argument_is_axis(const char * string)835 bool InputDevice::argument_is_axis(const char* string)
836 {
837 for (unsigned int i=0;i<sizeof(axesinfo)/sizeof(char*);i++)
838 if (stringncmp(string,axesinfo[i])==0)
839 return true;
840 if (stringncmp(string,"none")==0)
841 return true;
842 return false;
843 }
844
845 /*
846 * parse argumentstring
847 * valid syntax is
848 *
849 * -x|-y|-z|-xrot|-yrot|-zrot=[-][integer_axisnumber][,[factor][,[accel][,[wheel][,ignore]]]]
850 * -all|-allxyz|-allrot=[factor][,[accel][,[wheel][,ignore]]]
851 * -none=integer_axisnumber
852 *
853 * - sign of value from axis is swapped
854 *
855 * integer_axisnumber is a integer with the number of the axis, that should
856 * be used for the x y z xrot yrot zrot directions.
857 * This number may not be greater than the number of
858 * axes of the inputdevice
859 *
860 * factor is a float with a multiplicator for the axes
861 *
862 * accel is a float with a expotential accelerator for the axes
863 *
864 * wheel is the string "wheel"
865 * wheel means this axis of the inputdevice will not deliver zero
866 * if released
867 *
868 * ignore is a float with the value (relative to the maximal value
869 * from the device) which will be ignored (insensitivity)
870 *
871 * function returns always true
872 */
873
scan_argument(char * string,const char * argument)874 bool InputDevice::scan_argument(char *string,const char* argument)
875 {
876 int next;
877 bool sign_swap=false;
878 // skip argument
879 string=string+strlen(argument);
880
881 // skip '='
882 if (string[0]=='=')
883 string++;
884
885 // read sign of axis
886 if (string[0]=='-')
887 {
888 sign_swap=true;
889 string++;
890 }
891
892 if (argument_is_axis(argument))
893 {
894 // read axisnumber
895 next=cut_out_till_next_comma(string);
896 if (next!=1)
897 {
898 int axis_number;
899 if (!string2int(axis_number,string))
900 {
901 swDebugf("axisnumber %s is not a integer\n",string);
902 return true;
903 }
904 setAxisOfInformation(argument,axis_number);
905 if (strcmp(argument,"none")==0)
906 return true;
907 set_sign_swap(axis_number,sign_swap);
908 if (next==-1)
909 return true;
910 }
911 // position after comma
912 string=string+next;
913 }
914
915 // read factor
916 next=cut_out_till_next_comma(string);
917 if (next!=1)
918 {
919 unsigned int characters_read;
920 float fact;
921 sscanf(string,"%f%n",&fact,&characters_read);
922 if (characters_read!=strlen(string))
923 {
924 swDebugf("factor %s is not a float\n",string);
925 return true;
926 }
927 else
928 {
929 set_factor(argument,fact);
930 if (next==-1)
931 return true;
932 }
933 }
934 // position after comma
935 string=string+next;
936
937 // read acceleration
938 next=cut_out_till_next_comma(string);
939 if (next!=1)
940 {
941 unsigned int characters_read;
942 float accel;
943 sscanf(string,"%f%n",&accel,&characters_read);
944 if (characters_read!=strlen(string))
945 {
946 swDebugf("acceleration %s is not a float\n",string);
947 return true;
948 }
949 else
950 {
951 set_acceleration(argument,accel);
952 if (next==-1)
953 return true;
954 }
955 }
956 // position after comma
957 string=string+next;
958
959 // wheel/zero_on_release information
960 next=cut_out_till_next_comma(string);
961 if (next!=1)
962 {
963 if (strcmp(string,"wheel")==0)
964 set_zero_on_release(argument,false);
965 else
966 {
967 swDebugf("%s is not the string \"wheel\"\n",string);
968 return true;
969 }
970 if (next==-1)
971 return true;
972 } else
973 set_zero_on_release(argument,true);
974
975 // position after comma
976 string=string+next;
977
978 // read acceleration
979 next=cut_out_till_next_comma(string);
980 if (next==-1)
981 return true;
982 if (next!=1)
983 {
984 unsigned int characters_read;
985 float ignore;
986 sscanf(string,"%f%n",&ignore,&characters_read);
987 if (characters_read!=strlen(string))
988 {
989 swDebugf("zero_size_fraction %s is not a float\n",string);
990 return true;
991 }
992 else
993 {
994 set_zero_size_fraction(argument,ignore);
995 if (next==-1)
996 return true;
997 }
998 }
999 // position after comma
1000 string=string+next;
1001
1002 swDebugf("argument %s not understood\n",string);
1003 return true;
1004 }
1005
setAxisInformation(const char * info_string)1006 void InputDevice::setAxisInformation(const char* info_string)
1007 {
1008 char *string2free=strdup(info_string);
1009 char *string=string2free;
1010 if (string[0]=='-')
1011 string++;
1012
1013 bool found=false;
1014 for (unsigned int i=0;i<sizeof(axesinfo)/sizeof(char*);i++)
1015 {
1016 if (stringncmp(string,axesinfo[i])==0)
1017 {
1018 found=scan_argument(string,axesinfo[i]);
1019 break;
1020 }
1021 }
1022 if (stringncmp(string,"all")==0)
1023 {
1024 if (stringncmp(string,"allrot")==0)
1025 found=scan_argument(string,"allrot");
1026 else if (stringncmp(string,"allxyz")==0)
1027 found=scan_argument(string,"allxyz");
1028 else
1029 found=scan_argument(string,"all");
1030 }
1031 else if (stringncmp(string,"none")==0)
1032 found=scan_argument(string,"none");
1033
1034 if (!found)
1035 {
1036 swDebugf("%s do not contain",info_string);
1037 for (unsigned int i=0;i<sizeof(axesinfo)/sizeof(char*);i++)
1038 swDebugf(" -%s or",axesinfo[i]);
1039 swDebugf(" -all=wheel\n");
1040 }
1041 free(string2free);
1042 return;
1043 }
1044
1045 // set number of axes
1046 // only valid to reduce axes from a bad designed joystick
1047
setNumberAxes(const char * argument)1048 void InputDevice::setNumberAxes(const char* argument)
1049 {
1050 const char* string=argument;
1051
1052 if (stringncmp(string,"-axes")!=0)
1053 {
1054 swDebugf("internal error in InputDevice::SetNumberAxes\n");
1055 return;
1056 }
1057 string=string+strlen("-axes");
1058
1059 if (string[0]=='=')
1060 string++;
1061
1062 int new_number_axes;
1063 if (string2int(new_number_axes,string))
1064 {
1065 if (new_number_axes<=number_max_axis)
1066 number_max_axis=new_number_axes;
1067 else
1068 swDebugf("only %d axes found, can not decrease to %d\n",
1069 number_max_axis,new_number_axes);
1070 }
1071 return;
1072 }
1073
1074
checkUnixDevice(const char * device)1075 static bool checkUnixDevice(const char *device)
1076 {
1077 struct stat inode_info;
1078 if (stat(device,&inode_info)==0)
1079 #ifndef _WIN32
1080 if (S_ISCHR(inode_info.st_mode) || S_ISBLK(inode_info.st_mode))
1081 #endif
1082 return true;
1083 return false;
1084 }
1085
loopAddUnixDevices(StringArray * names,const char * device_base,int device_offset=0)1086 static void loopAddUnixDevices(StringArray *names, const char *device_base,
1087 int device_offset=0)
1088 {
1089 char device[128];
1090 int offset=device_offset;
1091 do
1092 {
1093 mysnprintf(device,127,"%s%d",device_base,offset);
1094 if (checkUnixDevice(device))
1095 {
1096 MyString append;
1097 append+=device;
1098 names->append(append);
1099 }
1100 offset++;
1101 } while (checkUnixDevice(device));
1102 }
1103
1104 // Drivers for joystick-like devices
1105
1106 // Linux Joystick driver of Linux 2.2 or Linux 2.4
1107
1108 /*
1109 * based on jstest.c Version 1.2
1110 *
1111 * Copyright (c) 1996-1999 Vojtech Pavlik
1112 *
1113 * Sponsored by SuSE
1114 *
1115 * modified by J. "MUFTI" Scheurich 2001-2002 for white_dune
1116 * This program can be used to test [..] the features of the Linux
1117 * joystick API, including non-blocking [..] access [..].
1118 * It is also intended to
1119 * serve as an example implementation for those who wish to learn
1120 * how to write their own joystick using applications.
1121 */
1122
1123 #ifdef LINUX_JOYSTICK
1124
1125 # include <sys/ioctl.h>
1126 # include <sys/time.h>
1127 # include <sys/types.h>
1128 # include <fcntl.h>
1129 # include <unistd.h>
1130 # include <errno.h>
1131
1132 /*
1133 * initialise joystick
1134 * argument "device" is the joystickdevice, eg. "/dev/input/js0" or "/dev/js0"
1135 */
1136
linux_joystick(const char * device)1137 linux_joystick::linux_joystick(const char *device) : InputDevice(device)
1138 {
1139 if (device == NULL)
1140 return;
1141
1142 if ((fd = open(device, O_RDONLY)) < 0)
1143 {
1144 valid = false;
1145 char msg[1024];
1146 mysnprintf(msg, 1024,
1147 "joystickdevice %s joystick initialisation failed: %s",
1148 device, strerror(errno));
1149 error_message += msg;
1150 }
1151 valid=true;
1152
1153 linux_js_factor=0.1;
1154 unsigned char axes;
1155 unsigned char buttons;
1156 number_max_axis = 2;
1157 number_buttons = 2;
1158 version = 0x000800;
1159 strcpy(name,"Unknown");
1160
1161 if (valid) {
1162 ioctl(fd, JSIOCGVERSION, &version);
1163 ioctl(fd, JSIOCGAXES, &axes);
1164 number_max_axis = axes;
1165 ioctl(fd, JSIOCGBUTTONS, &buttons);
1166 number_buttons = buttons;
1167 ioctl(fd, JSIOCGNAME(NAME_LENGTH), name);
1168 fcntl(fd, F_SETFL, O_NONBLOCK);
1169 }
1170
1171 if (number_buttons>0)
1172 {
1173 button=new bool[number_buttons];
1174 for (int i=0;i<number_buttons;i++)
1175 button[i]=false;
1176 }
1177
1178 # ifdef DEBUG
1179 swDebugf("Joystick (%s) has %d axes and %d buttons. Driver version is %d.%d.%d.\n",
1180 name, number_max_axis, number_buttons, version >> 16, (version >> 8) & 0xff,
1181 version & 0xff);
1182 # endif
1183 }
1184
~linux_joystick()1185 linux_joystick::~linux_joystick()
1186 {
1187 if (valid)
1188 close(fd);
1189 }
1190
1191 void
guessInputDeviceNames(StringArray * names) const1192 linux_joystick::guessInputDeviceNames(StringArray *names) const
1193 {
1194 names->resize(0);
1195 loopAddUnixDevices(names,"/dev/input/js");
1196 loopAddUnixDevices(names,"/dev/js");
1197 }
1198
readInputDevice(void)1199 bool linux_joystick::readInputDevice(void)
1200 {
1201 if (!valid)
1202 return false;
1203 bool ret=false;
1204 button_pressed=-1;
1205 button_released=-1;
1206 while (read(fd, &js, sizeof(struct js_event)) == sizeof(struct js_event))
1207 {
1208 # ifdef DEBUG
1209 swDebugf("Event: type %d, time %d, number %d, value %d\n",
1210 js.type, js.time, js.number, js.value);
1211 # endif
1212 switch(js.type & ~JS_EVENT_INIT)
1213 {
1214 case JS_EVENT_BUTTON:
1215 ret=true;
1216 if (js.value)
1217 {
1218 button[js.number] = true;
1219 button_pressed=js.number;
1220 }
1221 else
1222 {
1223 button[js.number] = false;
1224 button_released=js.number;
1225 }
1226 break;
1227 case JS_EVENT_AXIS:
1228 ret=true;
1229 if (js.number==num_axis_x)
1230 value[0] = js.value/maxvalue(js.number)*linux_js_factor;
1231 if (js.number==num_axis_y)
1232 value[1] = -js.value/maxvalue(js.number)*linux_js_factor;
1233 if (js.number==num_axis_z)
1234 value[2] = -js.value/maxvalue(js.number)*linux_js_factor;
1235 if (js.number==num_axis_xrot)
1236 value[3] = js.value/maxvalue(js.number)*linux_js_factor;
1237 if (js.number==num_axis_yrot)
1238 value[4] = js.value/maxvalue(js.number)*linux_js_factor;
1239 if (js.number==num_axis_zrot)
1240 value[5] = js.value/maxvalue(js.number)*linux_js_factor;
1241 break;
1242 }
1243 if ((button_pressed!=-1) || (button_released!=-1))
1244 break;
1245 }
1246 if (ret==false)
1247 if ((value[0]!=0) || (value[1]!=0) || (value[2]!=0) ||
1248 (value[3]!=0) || (value[4]!=0) || (value[5]!=0))
1249 {
1250 // deliver constantly values
1251 ret=true;
1252 }
1253 return ret;
1254 }
1255
1256 # ifdef TEST_JOYSTICK
main(int argc,char ** argv)1257 int main(int argc,char** argv)
1258 {
1259 linux_joystick js=linux_joystick("/dev/input/js0");
1260 while(1)
1261 if (js.readInputDevice())
1262 printf("%f %f %f\n",js.get_x(),js.get_y(),js.get_z());
1263 }
1264 # endif
1265 #endif
1266
1267 #ifdef _WIN32
appendNumbers(StringArray * names,int one2three=3)1268 static void appendNumbers(StringArray *names, int one2three=3)
1269 {
1270 names->resize(0);
1271
1272 MyString append;
1273 append="";
1274 append+="0";
1275 names->append(append);
1276
1277 if (one2three > 1)
1278 {
1279 append="";
1280 append+="1";
1281 names->append(append);
1282 }
1283
1284 if (one2three > 2)
1285 {
1286 append="";
1287 append+="2";
1288 names->append(append);
1289 }
1290 }
1291 #endif
1292
1293 #ifdef DIRECTX_JOYSTICK
1294
1295 # include <Windows.h>
1296
1297 BOOL CALLBACK EnumJoysticksCallback(const DIDEVICEINSTANCE* pdidInstance,
1298 VOID* pContext );
1299
1300 /*
1301 * initialise joystick
1302 * argument "device" is the number of the joystick, eg. "0", "1" or "2"
1303 */
1304
windows_joystick(const char * device)1305 windows_joystick::windows_joystick(const char *device) : InputDevice(device)
1306 {
1307 if (device == NULL)
1308 return;
1309
1310 int characters_read;
1311 sscanf(device,"%d%n",&number_of_joystick,&characters_read);
1312
1313 if ((characters_read!=strlen(device)) ||
1314 (number_of_joystick>15) || (number_of_joystick<0))
1315 {
1316 valid = false
1317 char msg[1024];
1318 mysnprintf(msg, 1024, "no joystick number \"%s\"",device);
1319 error_message += msg;
1320 }
1321
1322 if ( FAILED( InitDirectInput() ) )
1323 {
1324 valid = false
1325 char msg[1024];
1326 mysnprintf(msg, 1024,
1327 "joystick number \"%s\" not present or error", device);
1328 error_message += msg;
1329 }
1330
1331 DIDEVCAPS lpDIDevCaps;
1332 lpDIDevCaps.dwSize=sizeof(DIDEVCAPS);
1333 g_pJoystick->GetCapabilities(&lpDIDevCaps);
1334
1335 factor=1;
1336 rotfactor *= 0.1;
1337 number_max_axis=6;
1338 if (number_max_axis>0)
1339 {
1340 max_value=new int[6];
1341 null_value=new int[6];
1342
1343 for (int i=0;i<6;i++)
1344 {
1345 max_value[i]=65535;
1346 null_value[i]=32768;
1347 }
1348 }
1349
1350 number_buttons = lpDIDevCaps.dwButtons;
1351
1352 DIDEVICEINSTANCE pdidi;
1353 pdidi.dwSize=sizeof(DIDEVICEINSTANCE);
1354 g_pJoystick->GetDeviceInfo(&pdidi);
1355
1356 strcpy(name,pdidi.tszProductName);
1357
1358 DIPROPDWORD dipdw;
1359 dipdw.diph.dwSize = sizeof(DIPROPDWORD);
1360 dipdw.diph.dwHeaderSize = sizeof(DIPROPHEADER);
1361 dipdw.diph.dwObj = 0;
1362 dipdw.diph.dwHow = DIPH_DEVICE;
1363
1364 g_pJoystick->GetProperty(DIPROP_VIDPID,&dipdw.diph);
1365
1366 version = HIWORD(dipdw.dwData);
1367
1368 ///////////////////////////////////////////////////////////////////////////77
1369
1370 // version = joycapabilities.wPid;//?????????????????????????????????????????
1371
1372 /////////////////////////////////////////////////////////////////////////////
1373
1374 if (number_buttons>0)
1375 {
1376 button=new bool[number_buttons];
1377 for (int i=0;i<number_buttons;i++)
1378 button[i]=false;
1379 }
1380
1381 // ignore first read of joystick....
1382 // result=joyGetPosEx(number_of_joystick, &joyinfo_ex);
1383
1384 # ifdef DEBUG
1385 swDebugf("Joystick (%s) has %d axes and %d buttons. Driver version is %d.%d.%d.\n",
1386 name, number_max_axis, number_buttons, version >> 16, (version >> 8) & 0xff,
1387 version & 0xff);
1388 # endif
1389 }
1390
InitDirectInput()1391 HRESULT windows_joystick::InitDirectInput()
1392 {
1393 HRESULT hr;
1394 if ( FAILED( hr = DirectInput8Create( GetModuleHandle(NULL),
1395 DIRECTINPUT_VERSION,
1396 IID_IDirectInput8,
1397 (VOID**)&g_pDI, NULL ) ) )
1398 return hr;
1399
1400 // Look for a simple joystick we can use for this sample program.
1401 if ( FAILED( hr = g_pDI->EnumDevices( DI8DEVCLASS_GAMECTRL,
1402 EnumJoysticksCallback,
1403 NULL, DIEDFL_ATTACHEDONLY ) ) )
1404 return hr;
1405
1406 // Make sure we got a joystick
1407 if ( NULL == g_pJoystick )
1408 {
1409 MessageBox( NULL, TEXT("Joystick not found."),
1410 TEXT("DirectInput error"), SW_MB_ICONERROR | SW_MB_OK );
1411
1412 return S_OK;
1413 }
1414 // Set the data format to "simple joystick"
1415 if ( FAILED( hr = g_pJoystick->SetDataFormat( &c_dfDIJoystick2 ) ) )
1416 return hr;
1417
1418 // Would // Set the cooperative level to let DInput know how this device should
1419 // interact with the system and with other DInput applications.
1420 // if( FAILED( hr = g_pJoystick->SetCooperativeLevel( NULL/*hier hwnd*/, DISCL_EXCLUSIVE |
1421 // DISCL_FOREGROUND ) ) )
1422 // return hr;
1423
1424 DIPROPRANGE diprg;
1425 diprg.diph.dwSize = sizeof(DIPROPRANGE);
1426 diprg.diph.dwHeaderSize = sizeof(DIPROPHEADER);
1427 diprg.diph.dwHow = DIPH_DEVICE;//DIPH_BYID;
1428 diprg.diph.dwObj = 0;
1429 //pdidoi->dwType; // Specify the enumerated axis
1430 diprg.lMin = 0;
1431 diprg.lMax = +65535;
1432
1433 if ( FAILED( hr = g_pJoystick->SetProperty( DIPROP_RANGE, &diprg.diph ) ) )
1434 return hr;
1435
1436 return S_OK;
1437 }
1438
1439
~windows_joystick()1440 windows_joystick::~windows_joystick()
1441 {
1442 }
1443
1444 void
guessInputDeviceNames(StringArray * names) const1445 windows_joystick::guessInputDeviceNames(StringArray *names) const
1446 {
1447 appendNumbers(names);
1448 }
1449
EnumJoysticksCallback(const DIDEVICEINSTANCE * pdidInstance,VOID * pContext)1450 BOOL CALLBACK EnumJoysticksCallback( const DIDEVICEINSTANCE* pdidInstance,
1451 VOID* pContext )
1452 {
1453 HRESULT hr;
1454
1455 // Obtain an interface to the enumerated joystick.
1456 hr = g_pDI->CreateDevice( pdidInstance->guidInstance, &g_pJoystick, NULL );
1457
1458 // If it failed, then we can't use this joystick. (Maybe the user unplugged
1459 // it while we were in the middle of enumerating it.)
1460 if( FAILED(hr) )
1461 return DIENUM_CONTINUE;
1462
1463 // Stop enumeration. Note: we're just taking the first joystick we get. You
1464 // could store all the enumerated joysticks and let the user pick.
1465 return DIENUM_STOP;
1466 }
1467
1468
readInputDevice(void)1469 bool windows_joystick::readInputDevice(void)
1470 {
1471 if (NULL == g_pJoystick)
1472 return false;
1473
1474 if (FAILED(g_pJoystick->Poll()))
1475 {
1476 HRESULT hr;hr = g_pJoystick->Acquire();
1477 while ( hr == DIERR_INPUTLOST )
1478 hr = g_pJoystick->Acquire();
1479 return false;
1480 }
1481 if (! FAILED( g_pJoystick->GetDeviceState( sizeof(DIJOYSTATE2), &js ) ))
1482 {
1483 if (number_buttons>0)
1484 for (int i = 0; i < 128; i++ )
1485 {
1486 if ( js.rgbButtons[i] & 0x80 )
1487 button[i] = true;
1488 }
1489
1490 for (int i=0;i<6;i++)
1491 value[i]=0;
1492 value[0] = (float)((int)js.lX-null_value[0])/maxvalue(0)*factor;
1493 value[1] = -(float)((int)js.lY-null_value[1])/maxvalue(1)*factor;
1494 value[2] = (float)((int)js.lZ-null_value[2])/maxvalue(2)*factor;
1495 value[3] = (float)((int)js.rglSlider[0]-null_value[3])/maxvalue(3)*factor;
1496 value[4] = (float)((int)js.lRz-null_value[4])/maxvalue(4)*factor;
1497 value[5] = (float)((int)js.rgdwPOV[0]-null_value[5])/maxvalue(5)*factor;
1498 }
1499 else
1500 return false;
1501
1502 if ((value[0]!=0) || (value[1]!=0) || (value[2]!=0) ||
1503 (value[3]!=0) || (value[4]!=0) || (value[5]!=0))
1504 return true;
1505 return false;
1506 }
1507
1508 # ifdef TEST_JOYSTICK
main(int argc,char ** argv)1509 int main(int argc,char** argv)
1510 {
1511 windows_joystick js=windows_joystick("0");
1512 while(1)
1513 if (js.readInputDevice())
1514 printf("%f %f %f\n",js.get_x(),js.get_y(),js.get_z());
1515 }
1516 # endif
1517 #endif
1518
1519
1520 #ifdef WINDOWS_JOYSTICK
1521
1522 # include <Windows.h>
1523 # include "swt.h"
1524 /*
1525 * initialise joystick
1526 * argument "device" is the number of the joystick, eg. "0", "1" or "2"
1527 */
1528
windows_joystick(const char * device)1529 windows_joystick::windows_joystick(const char *device) : InputDevice(device)
1530 {
1531 if (device==NULL)
1532 return;
1533
1534 valid=true;
1535 device_name=device;
1536 int characters_read;
1537 sscanf(device,"%d%n",&number_of_joystick,&characters_read);
1538 if ((characters_read!=strlen(device)) ||
1539 (number_of_joystick>15) || (number_of_joystick<0))
1540 {
1541 valid=false;
1542 char msg[1024];
1543 mysnprintf(msg,1024,"no joystick number \"%s\"",device);
1544 error_message+=msg;
1545 return;
1546 }
1547 MMRESULT result;
1548 result=joyGetDevCaps(number_of_joystick,&joycapabilities,sizeof(JOYCAPS));
1549 if (result!=JOYERR_NOERROR)
1550 {
1551 valid=false;
1552 char msg[1024];
1553 mysnprintf(msg,1024,"joystick number \"%s\" not present or error",device);
1554 error_message+=msg;
1555 return;
1556 }
1557
1558 windows_js_factor=1;
1559 rotfactor*=0.1;
1560 if (valid)
1561 number_max_axis=joycapabilities.wNumAxes;
1562 else
1563 number_max_axis=0;
1564 if (number_max_axis>0)
1565 {
1566 maximum=new int[number_max_axis];
1567 null_value=new int[number_max_axis];
1568 for (int i=0;i<number_max_axis;i++)
1569 {
1570 switch(i)
1571 {
1572 case 0:
1573 maximum[i]=joycapabilities.wXmax;
1574 null_value[i]=(maximum[i]-joycapabilities.wXmin)/2;
1575 break;
1576 case 1:
1577 maximum[i]=joycapabilities.wYmax;
1578 null_value[i]=(maximum[i]-joycapabilities.wYmin)/2;
1579 break;
1580 case 2:
1581 maximum[i]=joycapabilities.wZmax;
1582 null_value[i]=(maximum[i]-joycapabilities.wZmin)/2;
1583 break;
1584 case 3:
1585 maximum[i]=joycapabilities.wRmax;
1586 null_value[i]=(maximum[i]-joycapabilities.wRmin)/2;
1587 break;
1588 case 4:
1589 maximum[i]=joycapabilities.wUmax;
1590 null_value[i]=(maximum[i]-joycapabilities.wUmin)/2;
1591 break;
1592 case 5:
1593 maximum[i]=joycapabilities.wVmax;
1594 null_value[i]=(maximum[i]-joycapabilities.wVmin)/2;
1595 break;
1596 }
1597 }
1598 }
1599
1600 if (valid) {
1601 number_buttons = joycapabilities.wMaxButtons;
1602 version = joycapabilities.wPid;
1603 strcpy(name,joycapabilities.szPname);
1604 }
1605
1606 if (number_buttons>0)
1607 {
1608 button=new bool[number_buttons];
1609 for (int i=0;i<number_buttons;i++)
1610 button[i]=false;
1611 }
1612
1613 // ignore first read of joystick....
1614 result=joyGetPosEx(number_of_joystick, &joyinfo_ex);
1615
1616
1617 # ifdef DEBUG
1618 swDebugf("Joystick (%s) has %d axes and %d buttons. Driver version is %d.%d.%d.\n",
1619 name, number_max_axis, number_buttons, version >> 16, (version >> 8) & 0xff,
1620 version & 0xff);
1621 # endif
1622 }
1623
~windows_joystick()1624 windows_joystick::~windows_joystick()
1625 {
1626 }
1627
1628 void
guessInputDeviceNames(StringArray * names) const1629 windows_joystick::guessInputDeviceNames(StringArray *names) const
1630 {
1631 appendNumbers(names);
1632 }
1633
readInputDevice(void)1634 bool windows_joystick::readInputDevice(void)
1635 {
1636 joyinfo_ex.dwSize=sizeof(JOYINFOEX);
1637 // joyinfo_ex.dwFlags=JOY_RETURNCENTERED ;// | JOY_RETURNBUTTONS;
1638 joyinfo_ex.dwFlags=JOY_RETURNALL | JOY_RETURNCENTERED; // | JOY_RETURNBUTTONS;
1639
1640 MMRESULT result=joyGetPosEx(number_of_joystick, &joyinfo_ex);
1641
1642 if (result == JOYERR_NOERROR)
1643 {
1644 if (number_buttons>0)
1645 for (int i=0;i<number_buttons;i++)
1646 button[i]=false;
1647 if (joyinfo_ex.dwFlags && JOY_RETURNBUTTONS)
1648 if (joyinfo_ex.dwButtons != 0)
1649 button[joyinfo_ex.dwButtonNumber] = true;
1650 for (int i=0;i<6;i++)
1651 value[i]=0;
1652 value[0] = (float)((int)joyinfo_ex.dwXpos-null_value[0])/maxvalue(0)*
1653 windows_js_factor;
1654 value[1] = - (float)((int)joyinfo_ex.dwYpos-null_value[1])/maxvalue(1)*
1655 windows_js_factor;
1656 if (joycapabilities.wCaps & JOYCAPS_HASZ)
1657 value[2] = - (float)((int)joyinfo_ex.dwZpos-null_value[2])/maxvalue(2)*
1658 windows_js_factor;
1659 if (joycapabilities.wCaps & JOYCAPS_HASR)
1660 value[3] = (float)((int)joyinfo_ex.dwRpos-null_value[3])/maxvalue(3)*
1661 windows_js_factor;
1662 if (joycapabilities.wCaps & JOYCAPS_HASU)
1663 value[4] = (float)((int)joyinfo_ex.dwUpos-null_value[4])/maxvalue(4)*
1664 windows_js_factor;
1665 if (joycapabilities.wCaps & JOYCAPS_HASV)
1666 value[5] = - (float)((int)joyinfo_ex.dwVpos-null_value[5])/maxvalue(5)*
1667 windows_js_factor;
1668 }
1669 else
1670 return false;
1671 if ((value[0]!=0) || (value[1]!=0) || (value[2]!=0) ||
1672 (value[3]!=0) || (value[4]!=0) || (value[5]!=0))
1673 return true;
1674 return false;
1675 }
1676
1677 # ifdef TEST_JOYSTICK
main(int argc,char ** argv)1678 int main(int argc,char** argv)
1679 {
1680 windows_joystick js=windows_joystick("0");
1681 while(1)
1682 if (js.readInputDevice())
1683 printf("%f %f %f\n",js.get_x(),js.get_y(),js.get_z());
1684 }
1685 # endif
1686 #endif
1687
1688 #ifdef HAVE_SDL_JOYSTICK
1689
1690 /*
1691 * initialise joystick
1692 * argument "device" is the number of the joystick, eg. "0", "1", "2", ...
1693 */
1694
SDL_joystick(const char * device)1695 SDL_joystick::SDL_joystick(const char *device) : InputDevice(device)
1696 {
1697 if (device == NULL)
1698 return;
1699
1700 int characters_read;
1701 sscanf(device,"%d%n",&number_of_joystick,&characters_read);
1702 js = NULL;
1703 if (SDL_JoystickInit() != 0)
1704 {
1705 valid = false;
1706 error_message += "joystick initialisation failed";
1707 return;
1708 }
1709 if (number_of_joystick >= SDL_NumJoysticks())
1710 {
1711 valid = false;
1712 char msg[1024];
1713 mysnprintf(msg, 1024, "joystickdevice %s only %d joysticks", device,
1714 SDL_NumJoysticks());
1715 error_message += msg;
1716 return;
1717 }
1718
1719 js = NULL;
1720 if (valid)
1721 js = SDL_JoystickOpen(number_of_joystick);
1722 sdl_factor = 1;
1723 rotfactor *= 0.1;
1724 if (valid)
1725 number_max_axis = SDL_JoystickNumAxes(js);
1726 else
1727 number_max_axis = 2;
1728 int array_size = number_max_axis < 6 ? 6 : number_max_axis;
1729 maximum=new int[array_size];
1730 null_value=new int[array_size];
1731 for (int i=0;i<array_size;i++)
1732 {
1733 maximum[i]=256*256;
1734 null_value[i]=0;
1735 }
1736 number_buttons = 0;
1737 }
1738
~SDL_joystick()1739 SDL_joystick::~SDL_joystick()
1740 {
1741 if (js != NULL)
1742 SDL_JoystickClose(js);
1743 }
1744
1745 void
guessInputDeviceNames(StringArray * names) const1746 SDL_joystick::guessInputDeviceNames(StringArray *names) const
1747 {
1748 appendNumbers(names);
1749 }
1750
readInputDevice(void)1751 bool SDL_joystick::readInputDevice(void)
1752 {
1753 bool ret=false;
1754
1755 SDL_JoystickUpdate();
1756 if (num_axis_x < number_max_axis)
1757 value[0] = ((float)SDL_JoystickGetAxis(js, num_axis_x)) /
1758 maxvalue(num_axis_x) * sdl_factor;
1759 if (num_axis_y < number_max_axis)
1760 value[1] = ((float)SDL_JoystickGetAxis(js, num_axis_y)) /
1761 maxvalue(num_axis_y) * sdl_factor;
1762 if (num_axis_z < number_max_axis)
1763 value[2] = ((float)SDL_JoystickGetAxis(js, num_axis_z)) /
1764 maxvalue(num_axis_z) * sdl_factor;
1765
1766 if (num_axis_xrot < number_max_axis)
1767 value[3] = ((float)SDL_JoystickGetAxis(js, num_axis_xrot)) /
1768 maxvalue(num_axis_xrot) * sdl_factor;
1769 if (num_axis_yrot < number_max_axis)
1770 value[4] = ((float)SDL_JoystickGetAxis(js, num_axis_yrot)) /
1771 maxvalue(num_axis_yrot) * sdl_factor;
1772 if (num_axis_zrot < number_max_axis)
1773 value[5] = ((float)SDL_JoystickGetAxis(js, num_axis_zrot)) /
1774 maxvalue(num_axis_zrot) * sdl_factor;
1775
1776 if ((value[0]!=0) || (value[1]!=0) || (value[2]!=0) ||
1777 (value[3]!=0) || (value[4]!=0) || (value[5]!=0))
1778 {
1779 // deliver constantly values
1780 ret=true;
1781 }
1782 return ret;
1783 }
1784 #endif
1785
1786 // Xinput driver
1787
1788 #ifdef HAVE_XINPUT
1789
1790 /*
1791 * initialise XInput
1792 * argument "device" is the name of the device, eg. "magellan"
1793 */
1794
xinput(const char * device)1795 xinput::xinput(const char *device) : InputDevice(device)
1796 {
1797 if (device==NULL)
1798 return;
1799
1800 number_buttons=0;
1801 number_max_axis=0;
1802 xinput_factor=1;
1803 maximum=NULL;
1804
1805 swxinput=(swXinput*) malloc(sizeof(swXinput));
1806 swInitXinputDevice(swxinput,device);
1807
1808 if (swxinput->xinput==NULL)
1809 {
1810 char msg[1024];
1811 mysnprintf(msg,1024,"failed to initialize xinput device %s",device);
1812 error_message+=msg;
1813 free(swxinput);
1814 swxinput=NULL;
1815 valid=false;
1816 return;
1817 }
1818
1819 if (swxinput)
1820 number_buttons=swxinput->number_buttons;
1821 else
1822 number_buttons = 0;
1823 if (number_buttons>0)
1824 {
1825 button=new bool[number_buttons];
1826 for (int i=0;i<number_buttons;i++)
1827 button[i]=false;
1828 }
1829
1830 if (swxinput)
1831 number_max_axis=swxinput->number_axes;
1832 else
1833 number_max_axis=0;
1834
1835 if (number_max_axis>0)
1836 {
1837 int array_size = number_max_axis < 7 ? 7 : number_max_axis;
1838 maximum=new float[array_size];
1839 for (int i=0;i<number_max_axis;i++)
1840 maximum[i]=swxinput->maxvalue[i];
1841 for (int i=number_max_axis;i<7;i++)
1842 maximum[i]=1;
1843 }
1844
1845 name=(char *)"Unknown";
1846
1847 # ifdef DEBUG
1848 swDebugf("Xinputdevice %s has %d axes and %d buttons.\n",
1849 device, number_max_axis, number_buttons);
1850 # endif
1851 }
1852
~xinput(void)1853 xinput::~xinput(void)
1854 {
1855 swCloseXinputDevice(swxinput);
1856 free(swxinput);
1857 }
1858
1859 void
guessInputDeviceNames(StringArray * names) const1860 xinput::guessInputDeviceNames(StringArray *names) const
1861 {
1862 names->resize(0);
1863 MyString append;
1864 for (int i=0;i<swGetNumXinputDevice();i++)
1865 {
1866 append="";
1867 append+=swGetXinputDevice(i);
1868 names->append(append);
1869 }
1870 }
1871
readInputDevice(void)1872 bool xinput::readInputDevice(void)
1873 {
1874 button_pressed=-1;
1875 button_released=-1;
1876
1877 if (swxinput==NULL)
1878 return false;
1879 for (int i=0;i<number_buttons;i++)
1880 button[i]=false;
1881
1882 swQueryXinputDevice(swxinput);
1883 if (swxinput->xinput==NULL)
1884 return false;
1885
1886 for (int j=0;j<7;j++)
1887 value[j] = 0;
1888
1889 if (num_axis_x < number_max_axis)
1890 value[0]= xinput_factor*float(swxinput->axes[num_axis_x])/
1891 swxinput->maxvalue[num_axis_x];
1892 if (num_axis_y < number_max_axis)
1893 value[1]= xinput_factor*float(swxinput->axes[num_axis_y])/
1894 swxinput->maxvalue[num_axis_y];
1895 if (num_axis_z < number_max_axis)
1896 value[2]=-xinput_factor*float(swxinput->axes[num_axis_z])/
1897 swxinput->maxvalue[num_axis_z];
1898
1899 if (num_axis_xrot < number_max_axis)
1900 value[3]= xinput_factor*float(swxinput->axes[num_axis_xrot])/
1901 swxinput->maxvalue[num_axis_xrot];
1902 if (num_axis_yrot < number_max_axis)
1903 value[4]= xinput_factor*float(swxinput->axes[num_axis_yrot])/
1904 swxinput->maxvalue[num_axis_yrot];
1905 if (num_axis_zrot < number_max_axis)
1906 value[5]= xinput_factor*float(swxinput->axes[num_axis_zrot])/
1907 swxinput->maxvalue[num_axis_zrot];
1908 if (num_axis_angle < number_max_axis)
1909 value[6]= xinput_factor*float(swxinput->axes[num_axis_angle])/
1910 swxinput->maxvalue[num_axis_angle];
1911
1912 return true;
1913 }
1914 #endif
1915
1916 // Driver for SpaceTec/LabTec Spaceball and clones.
1917
1918 #ifdef HAVE_LIBSBALL
1919
1920 /*
1921 * initialise spaceball
1922 * argument "device" is the spaceballdevice, eg. "/dev/ttyd2" or "/dev/ttyS0"
1923 */
1924
spaceball(const char * device)1925 spaceball::spaceball(const char *device) : InputDevice(device)
1926 {
1927 if (device == NULL)
1928 return;
1929
1930 sball = NULL;
1931 if ((sball = sball_open((char *)device)) == NULL)
1932 {
1933 char msg[1024];
1934 mysnprintf(msg,1024,"spaceballdevice %s initialisation failed",device);
1935 error_message+=msg;
1936 valid=false;
1937 }
1938
1939 if (valid)
1940 sball_init(sball);
1941 nullsize = 0;
1942 maximum = 32767.0;
1943 sball_factor = 1.0;
1944
1945 if (valid)
1946 sball_set_nullregion(sball, nullsize, nullsize, nullsize,
1947 nullsize, nullsize, nullsize);
1948
1949 number_max_axis = 6;
1950 number_buttons = 9;
1951 name = "Unknown Spaceball";
1952
1953 if (number_buttons>0)
1954 {
1955 button = new bool[number_buttons];
1956 for (int i = 0; i < number_buttons; i++)
1957 button[i] = false;
1958 }
1959
1960 # ifdef DEBUG
1961 swDebugf("InputDevice (%s) has %d axes and %d buttons. Driver ???\n",
1962 name, number_max_axis, number_buttons);
1963 # endif
1964 }
1965
~spaceball(void)1966 spaceball::~spaceball(void)
1967 {
1968 sball_close(sball);
1969 }
1970
1971 void
guessInputDeviceNames(StringArray * names) const1972 spaceball::guessInputDeviceNames(StringArray *names) const
1973 {
1974 names->resize(0);
1975 #ifdef SERIAL_DEVICE
1976 # ifdef OFFSET_SERIAL_DEVICE
1977 loopAddUnixDevices(names,SERIAL_DEVICE,OFFSET_SERIAL_DEVICE);
1978 # endif
1979 #endif
1980 loopAddUnixDevices(names,"/dev/ttyUSB");
1981 }
1982
readInputDevice(void)1983 bool spaceball::readInputDevice(void)
1984 {
1985 bool ret=false;
1986 button_pressed=-1;
1987 button_released=-1;
1988 int buttons;
1989 int sx,sy,sz;
1990 int sxrot,syrot,szrot;
1991
1992 if (sball==NULL)
1993 return false;
1994 for (int i=0;i<number_buttons;i++)
1995 button[i]=false;
1996 if (sball_getstatus(sball, &sx, &sy, &sz, &sxrot, &syrot, &szrot, &buttons))
1997 {
1998 value[0]= ((float)sx)/maxvalue(0)*sball_factor;
1999 value[1]= ((float)sy)/maxvalue(1)*sball_factor;
2000 value[2]=-((float)sz)/maxvalue(2)*sball_factor;
2001 value[3]= ((float)sxrot)/maxvalue(3)*sball_factor;
2002 value[4]= ((float)syrot)/maxvalue(4)*sball_factor;
2003 value[5]= ((float)szrot)/maxvalue(5)*sball_factor;
2004 ret=true;
2005 }
2006 return ret;
2007 }
2008
2009 # ifdef TEST_SPACEBALL
main(int argc,char ** argv)2010 int main(int argc,char** argv)
2011 {
2012 spaceball device=spaceball("/dev/ttyd2");
2013 while(1)
2014 if (device.readInputDevice())
2015 printf("%f %f %f\n",device.get_x(),device.get_y(),device.get_z());
2016 }
2017 # endif
2018 #endif
2019
2020 #ifdef HAVE_NXT_DIALS
2021 #include <usb.h>
2022 // USB functions almost unmodified from libnxtusb.cpp of
2023 // http://jgraef.bplaced.de/libnxtusb-0.1b.zip
2024 /********************************************************************************
2025 * LibNXTUSB *
2026 ********************************************************************************
2027 * *
2028 * Name: LibNXTUSB 0.1 *
2029 * Auhor: Janosch Gräf <jgraef@users.sf.net> *
2030 * License: GNU/GPL *
2031 * *
2032 ********************************************************************************
2033 * *
2034 * LibNXTUSB - Use your NXT with USB under Linux *
2035 * Copyright (C) 2007 Janosch Gräf *
2036 * *
2037 * This program is free software: you can redistribute it and/or modify *
2038 * it under the terms of the GNU General Public License as published by *
2039 * the Free Software Foundation, either version 3 of the License, or *
2040 * (at your option) any later version. *
2041 * *
2042 * This program is distributed in the hope that it will be useful, *
2043 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
2044 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
2045 * GNU General Public License for more details. *
2046 * *
2047 * You should have received a copy of the GNU General Public License *
2048 * along with this program. If not, see <http://www.gnu.org/licenses/>. *
2049 * *
2050 ********************************************************************************/
2051
2052 static int usbUsageCount = 0;
2053
nxtDials(const char * device)2054 nxtDials::nxtDials(const char* device) : InputDevice(device) {
2055 if (device == NULL)
2056 return;
2057
2058 valid=true;
2059 number_max_axis=3;
2060 number_max_axes=3;
2061 // set "wheel" property
2062 for (int i=0;i<number_max_axes;i++)
2063 set_zero_on_release(i,false);
2064 maximum=32767.0;
2065 factor=100.0;
2066 errorString="";
2067 if (usbUsageCount==0) {
2068 usb_init();
2069 usb_set_debug(0);
2070 }
2071 usbUsageCount++;
2072 int number_nxt = 0;
2073 if (device != NULL)
2074 number_nxt = atoi(device);
2075 nxt_handle=locate_nxt(number_nxt);
2076 if (!nxt_handle) {
2077 errorString="locate_nxt failed";
2078 return;
2079 }
2080 if (usb_reset(nxt_handle)) {
2081 errorString="Could not reset USB";
2082 valid=false;
2083 } else if (usb_set_configuration(nxt_handle,USB_NXT_CONFIG)) {
2084 errorString="Could not set configuration to 1";
2085 valid=false;
2086 } else if (usb_claim_interface(nxt_handle,USB_INTERFACE)) {
2087 errorString="Could not claim interface to 0";
2088 valid=false;
2089 } else if (usb_set_altinterface(nxt_handle,USB_INTERFACE)) {
2090 errorString="Could not set alternative interface 0";
2091 valid=false;
2092 }
2093 if (!valid)
2094 error_message+=errorString;
2095 }
2096
~nxtDials()2097 nxtDials::~nxtDials()
2098 {
2099 usbUsageCount--;
2100 if (usbUsageCount==0)
2101 shutdown();
2102 }
2103
2104 void
guessInputDeviceNames(StringArray * names) const2105 nxtDials::guessInputDeviceNames(StringArray *names) const
2106 {
2107 names->resize(0);
2108
2109 MyString append;
2110 append="";
2111 append+="NXT";
2112 names->append(append);
2113 }
2114
readInputDevice(void)2115 bool nxtDials::readInputDevice(void) {
2116 if (!valid)
2117 return false;
2118 value[0]=(getRotationCount(0)/maxvalue(0))*factor;
2119 value[1]=(getRotationCount(1)/maxvalue(1))*factor;
2120 value[2]=(getRotationCount(2)/maxvalue(2))*factor;
2121 return true;
2122 }
2123
2124 // USB functions
shutdown()2125 void nxtDials::shutdown() {
2126 if (nxt_handle!=NULL) {
2127 usb_release_interface(nxt_handle,USB_INTERFACE);
2128 usb_close(nxt_handle);
2129 }
2130 }
2131
locate_nxt(int index)2132 usb_dev_handle *nxtDials::locate_nxt(int index) {
2133 struct usb_bus *bus;
2134 struct usb_device *dev;
2135
2136 usb_find_busses();
2137 usb_find_devices();
2138 for (bus = usb_busses; bus; bus = bus->next) {
2139 for (dev = bus->devices; dev; dev = dev->next) {
2140 if ((dev->descriptor.idVendor == USB_ID_VENDOR_LEGO) &&
2141 (dev->descriptor.idProduct == USB_ID_PRODUCT_NXT)) {
2142 if (index==0) return usb_open(dev);
2143 else index--;
2144 }
2145 }
2146 }
2147
2148 swDebugf("[USB] No device found\n");
2149 valid = false;
2150 return NULL;
2151 }
2152
usb_sendpacket(char * buffer,int size)2153 int nxtDials::usb_sendpacket(char *buffer,int size) {
2154 int result;
2155 result = usb_bulk_write(nxt_handle,USB_OUT_ENDPOINT,buffer,size,USB_TIMEOUT);
2156 if (result<0) {
2157 result *= -1;
2158 swDebugf("[USB] Send error [%i]: %s\r\n",result,strerror(result));
2159 valid = false;
2160 }
2161 return result;
2162 }
2163
usb_recvpacket(char * buffer,int size)2164 int nxtDials::usb_recvpacket(char *buffer,int size) {
2165 int result;
2166 result = usb_bulk_read(nxt_handle,USB_IN_ENDPOINT,buffer,size,USB_TIMEOUT);
2167 if (result<0) {
2168 result *= -1;
2169 swDebugf("[USB] Receive error [%i]: %s\r\n",result,strerror(result));
2170 valid = false;
2171 }
2172 return result;
2173 }
2174
abortprog()2175 void nxtDials::abortprog() { /// TODO: recv response
2176 char buffer[64];
2177 buffer[0] = 0x80;
2178 buffer[1] = 0x01;
2179 usb_sendpacket(buffer,2);
2180 }
2181
getRotationCount(int motorport)2182 int nxtDials::getRotationCount(int motorport) {
2183 unsigned char buffer[64];
2184 buffer[0] = 0x00;
2185 buffer[1] = 0x06;
2186 buffer[2] = motorport;
2187 usb_sendpacket((char*)buffer,3);
2188 usb_recvpacket((char*)buffer,25);
2189
2190 int count = buffer[24] * 0x1000000;
2191 count += buffer[23] * 0x10000;
2192 count += buffer[22] * 0x100;
2193 count += buffer[21];
2194
2195 return count;
2196 }
2197 #endif
2198
2199 #ifdef HAVE_WINDOWS_P5
2200
2201 /*
2202 * initialise p5
2203 * argument "device" is the number of the p5 device, eg. "0" or "1"
2204 */
2205
windowsp5(const char * device)2206 windowsp5::windowsp5(const char *device) : InputDevice(device)
2207 {
2208 if (device==NULL)
2209 return;
2210
2211 device_name=device;
2212 valid=true;
2213 int characters_read;
2214 number_max_axis = 6;
2215 sscanf(device,"%d%n",&number_p5,&characters_read);
2216 if (!(bP5Present=P5.P5_Init()))
2217 {
2218 swDebugf("windows_p5_device %s ",device);
2219 swDebugf("initialisation failed\n");
2220 valid=false;
2221 return;
2222 }
2223 else
2224 {
2225 P5Bend_Init(&P5, number_p5);
2226 P5Motion_Init(&P5,number_p5);
2227 };
2228
2229 nullsize=0;
2230 maximum=256.0;
2231 p5_factor=1.0;
2232 // the P5 is a "wheel" type device
2233 for (int i=0;i<number_max_axis;i++)
2234 set_zero_on_release(i,false);
2235 number_buttons = 9;
2236 name="Unknown windowsp5";
2237
2238 if (number_buttons>0)
2239 {
2240 button=new bool[number_buttons];
2241 for (int i=0;i<number_buttons;i++)
2242 button[i]=false;
2243 }
2244
2245 # ifdef DEBUG
2246 swDebugf("InputDevice (%s) has %d axes and %d buttons. Driver ???\n",
2247 name, number_max_axis, number_buttons);
2248 # endif
2249 }
2250
~windowsp5(void)2251 windowsp5::~windowsp5(void)
2252 {void P5_Close();}
2253
2254 void
guessInputDeviceNames(StringArray * names) const2255 windowsp5::guessInputDeviceNames(StringArray *names) const
2256 {
2257 appendNumbers(names,1);
2258 }
2259
readInputDevice(void)2260 bool windowsp5::readInputDevice(void)
2261 {
2262 if (bP5Present)
2263 {
2264 int i=0;
2265 button[i++]=(((unsigned char)P5.m_P5Devices[number_p5].m_byBendSensor_Data[P5_THUMB]) > 33);
2266 button[i++]=(((unsigned char)P5.m_P5Devices[number_p5].m_byBendSensor_Data[P5_INDEX]) > 33);
2267 button[i++]=(((unsigned char)P5.m_P5Devices[number_p5].m_byBendSensor_Data[P5_MIDDLE]) > 33);
2268 button[i++]=(((unsigned char)P5.m_P5Devices[number_p5].m_byBendSensor_Data[P5_RING]) > 33);
2269 button[i++]=(((unsigned char)P5.m_P5Devices[number_p5].m_byBendSensor_Data[P5_PINKY]) > 33);
2270 button[i++]=P5.m_P5Devices[number_p5].m_byButtons[0];
2271 button[i++]=P5.m_P5Devices[number_p5].m_byButtons[1];
2272 button[i++]=P5.m_P5Devices[number_p5].m_byButtons[2];
2273 button[i++]=P5.m_P5Devices[number_p5].m_byButtons[3];
2274
2275 value[0]= (float)P5.m_P5Devices[number_p5].m_fx/maxvalue(0)*p5_factor;
2276 value[1]= (float)P5.m_P5Devices[number_p5].m_fy/maxvalue(1)*p5_factor;
2277 value[2]=-(float)P5.m_P5Devices[number_p5].m_fz/maxvalue(2)*p5_factor;
2278 value[3]= (float)(P5.m_P5Devices[number_p5].m_froll)/maxvalue(3)*p5_factor;
2279 value[4]= (float)(P5.m_P5Devices[number_p5].m_fyaw)/maxvalue(4)*p5_factor;
2280 value[5]= (float)(P5.m_P5Devices[number_p5].m_fpitch)/maxvalue(5)*p5_factor;
2281 }
2282 return bP5Present;
2283 }
2284
2285 # ifdef TEST_P5
main(int argc,char ** argv)2286 int main(int argc,char** argv)
2287 {
2288 windowsp5 device=windowsp5("/dev/ttyd2");
2289 while(1)
2290 if (device.readInputDevice())
2291 printf("%f %f %f\n",device.get_x(),device.get_y(),device.get_z());
2292 }
2293 # endif
2294 #endif
2295
2296 // M$Windows Driver for SpaceTec/LabTec Spaceball and clones.
2297
2298 #ifdef HAVE_WINDOWS_SPACEBALL
2299 /*
2300 * initialise spaceball
2301 * argument "device" is useless
2302 */
2303
2304 #include "swt.h"
2305
windowsspaceball(const char * device)2306 windowsspaceball::windowsspaceball(const char *device) : InputDevice(device)
2307 {
2308 if (device == NULL)
2309 return;
2310
2311 device_name = device;
2312 valid = true;
2313 initialized = swSpaceNavInit();
2314 if (!initialized)
2315 {
2316 char msg[1024];
2317 mysnprintf(msg, 1024, "windowsspaceballdevice %s initialisation failed",
2318 device);
2319 error_message += msg;
2320 valid = false;
2321 return;
2322 }
2323
2324 nullsize=0;
2325 maximum=32767.0;
2326 windows_sball_factor=10.0;
2327
2328 number_max_axis = 6;
2329 number_buttons = 9;
2330 name="Unknown windowsspaceball";
2331
2332 if (number_buttons>0)
2333 {
2334 button=new bool[number_buttons];
2335 for (int i=0;i<number_buttons;i++)
2336 button[i]=false;
2337 }
2338
2339 # ifdef DEBUG
2340 swDebugf("InputDevice (%s) has %d axes and %d buttons. Driver ???\n",
2341 name, number_max_axis, number_buttons);
2342 # endif
2343 }
2344
~windowsspaceball(void)2345 windowsspaceball::~windowsspaceball(void)
2346 {
2347 swSpaceNavFree();
2348 }
2349
2350 void
guessInputDeviceNames(StringArray * names) const2351 windowsspaceball::guessInputDeviceNames(StringArray *names) const
2352 {
2353 appendNumbers(names,1);
2354 }
2355
readInputDevice(void)2356 bool windowsspaceball::readInputDevice(void)
2357 {
2358 bool ret=false;
2359 int sx,sy,sz;
2360 int sxrot,syrot,szrot;
2361
2362 if (!initialized)
2363 return false;
2364
2365 sx=swSpaceNavGetData(0);
2366 sz=swSpaceNavGetData(1);
2367 sy=swSpaceNavGetData(2);
2368 sxrot=swSpaceNavGetData(3);
2369 szrot=swSpaceNavGetData(4);
2370 syrot=swSpaceNavGetData(5);
2371
2372 value[0]= ((float)sx)/maxvalue(0)*windows_sball_factor;
2373 value[1]= -((float)sy)/maxvalue(1)*windows_sball_factor;
2374 value[2]= ((float)sz)/maxvalue(2)*windows_sball_factor;
2375 value[3]= ((float)sxrot)/maxvalue(3)*windows_sball_factor;
2376 value[4]= -((float)syrot)/maxvalue(4)*windows_sball_factor;
2377 value[5]= -((float)szrot)/maxvalue(5)*windows_sball_factor;
2378
2379 return true;
2380 }
2381
2382 # ifdef TEST_SPACEBALL
main(int argc,char ** argv)2383 int main(int argc,char** argv)
2384 {
2385 windowsspaceball device=windowsspaceball("/dev/ttyd2");
2386 while(1)
2387 if (device.readInputDevice())
2388 printf("%f %f %f\n",device.get_x(),device.get_y(),device.get_z());
2389 }
2390 # endif
2391 #endif
2392
2393
2394
2395 // Driver for Ascention Flock of birds devices
2396
2397 #ifdef HAVE_AFLOCK
2398 #include <unistd.h>
2399
2400 /*
2401 * initialise Aflock
2402 */
2403
AflockDevice(const char * dev)2404 AflockDevice::AflockDevice(const char* dev)
2405 {
2406 /* insert your default configuration here */
2407 device = dev;
2408 errorMessage="";
2409 if (device==NULL)
2410 return;
2411
2412 baudrate = 38400; // baudrate of device
2413 sync = 0;
2414 block = 0;
2415 numBrds = 2; // number of birds (without transmitter)
2416 transmit = 1; // number of bird transmitter unit
2417 hemi = RIGHT_HEM; // hemisphere (sit on the antenna block to see,
2418 // what is left or right 8-)
2419 filt = FILTER_DEFAULT;// use default filter set by flock autoconfig
2420 sudden_change_lock=true;
2421 report = 'Q';
2422 calfile = (char *)""; // calibration file
2423 /* end of configuratable data */
2424
2425 ignoreSize = 0;
2426
2427 opened=false;
2428 flock=NULL;
2429 }
2430
~AflockDevice(void)2431 AflockDevice::~AflockDevice(void)
2432 {
2433 if (flock!=NULL)
2434 if (opened)
2435 {
2436 flock->stop();
2437 opened=false;
2438 delete flock;
2439 flock=NULL;
2440 }
2441 }
2442
setBaud(char * baudrate_string)2443 void AflockDevice::setBaud(char* baudrate_string)
2444 {
2445 if (!string2int(baudrate,baudrate_string))
2446 swDebugf("argument %s is not a integer, ignored\n",baudrate_string);
2447 }
2448
setSync(char * sync_string)2449 void AflockDevice::setSync(char* sync_string)
2450 {
2451 if (!string2int(sync,sync_string))
2452 swDebugf("argument %s is not a integer, ignored\n",sync_string);
2453 }
2454
setBlock(char * block_string)2455 void AflockDevice::setBlock(char* block_string)
2456 {
2457 if (!string2int(block,block_string))
2458 swDebugf("argument %s is not a integer, ignored\n",block_string);
2459 }
2460
setNumBrds(char * numBrds_string)2461 void AflockDevice::setNumBrds(char* numBrds_string)
2462 {
2463 if (!string2int(numBrds,numBrds_string))
2464 swDebugf("argument %s is not a integer, ignored\n",numBrds_string);
2465 }
2466
setTransmit(char * transmit_string)2467 void AflockDevice::setTransmit(char* transmit_string)
2468 {
2469 if (!string2int(transmit,transmit_string))
2470 swDebugf("argument %s is not a integer, ignored\n",transmit_string);
2471 }
2472
setHemi(char * hemi_string)2473 void AflockDevice::setHemi(char* hemi_string)
2474 {
2475 if (strcmp(hemi_string,"FRONT_HEM")==0)
2476 hemi=FRONT_HEM;
2477 else if (strcmp(hemi_string,"AFT_HEM")==0)
2478 hemi=AFT_HEM;
2479 else if (strcmp(hemi_string,"UPPER_HEM")==0)
2480 hemi=UPPER_HEM;
2481 else if (strcmp(hemi_string,"LOWER_HEM")==0)
2482 hemi=LOWER_HEM;
2483 else if (strcmp(hemi_string,"LEFT_HEM")==0)
2484 hemi=LEFT_HEM;
2485 else if (strcmp(hemi_string,"RIGHT_HEM")==0)
2486 hemi=RIGHT_HEM;
2487 else
2488 {
2489 swDebugf("argument %s is no of ",hemi_string);
2490 swDebugf("FRONT_HEM, AFT_HEM, UPPER_HEM, LOWER_HEM,");
2491 swDebugf(" LEFT_HEM, RIGHT_HEM, ignored\n");
2492 }
2493 }
2494
getHemi(void)2495 const char* AflockDevice::getHemi(void)
2496 {
2497 if (hemi==FRONT_HEM)
2498 return("FRONT_HEM");
2499 else if (hemi==AFT_HEM)
2500 return("AFT_HEM");
2501 else if (hemi==UPPER_HEM)
2502 return("UPPER_HEM");
2503 else if (hemi==LOWER_HEM)
2504 return("LOWER_HEM");
2505 else if (hemi==LEFT_HEM)
2506 return("LEFT_HEM");
2507 else if (hemi==RIGHT_HEM)
2508 return("RIGHT_HEM");
2509 else
2510 {
2511 swDebugf("argument %d is no of ",hemi);
2512 swDebugf("FRONT_HEM, AFT_HEM, UPPER_HEM, LOWER_HEM,");
2513 swDebugf(" LEFT_HEM, RIGHT_HEM, ignored\n");
2514 return("");
2515 }
2516 }
2517
2518
setFilt(char * filt_string)2519 void AflockDevice::setFilt(char* filt_string)
2520 {
2521 if (strcmp(filt_string,"AC_NARROW")==0)
2522 filt=filt | AC_NARROW;
2523 else if (strcmp(filt_string,"AC_WIDE")==0)
2524 filt=filt | AC_WIDE;
2525 else if (strcmp(filt_string,"DC_FILTER")==0)
2526 filt=filt | DC_FILTER;
2527 else
2528 {
2529 swDebugf("argument %s is no of ",filt_string);
2530 swDebugf("AC_NARROW, AC_WIDE, DC_FILTER, ignored\n");
2531 }
2532 }
2533
getFilt(void)2534 const char* AflockDevice::getFilt(void)
2535 {
2536 if (filt & AC_NARROW)
2537 return("AC_NARROW");
2538 else if (filt & AC_WIDE)
2539 return("AC_WIDE");
2540 else if (filt & DC_FILTER)
2541 return("DC_FILTER");
2542 else
2543 {
2544 swDebugf("argument %d is no of ",filt);
2545 swDebugf("AC_NARROW, AC_WIDE, DC_FILTER, ignored\n");
2546 return "";
2547 }
2548 }
2549
setSuddenChangeLock(char * sudden_string)2550 void AflockDevice::setSuddenChangeLock(char* sudden_string)
2551 {
2552 int sudden=1;
2553 if (!string2int(sudden,sudden_string))
2554 swDebugf("argument %s is not a integer, ignored\n",sudden_string);
2555 if (sudden==0)
2556 sudden_change_lock=false;
2557 else
2558 sudden_change_lock=true;
2559 }
2560
setReport(char * report_string)2561 void AflockDevice::setReport(char* report_string)
2562 {
2563 report=report_string[0];
2564 }
2565
setCalfile(char * calfile_string)2566 void AflockDevice::setCalfile(char* calfile_string)
2567 {
2568 calfile=calfile_string;
2569 }
2570
2571
2572 /* set device and initialise flock of birds */
2573
getAflock(void)2574 Aflock* AflockDevice::getAflock(void)
2575 {
2576 if (!opened)
2577 {
2578 flock=NULL;
2579 flock = new Aflock(device,baudrate,sync,block,
2580 numBrds,transmit,hemi,filt,sudden_change_lock,
2581 report,calfile);
2582 if (!flock->start())
2583 {
2584 flock=NULL;
2585 swDebugf("Aflock %s ",device);
2586 swDebugf("initialisation failed\n");
2587 return flock;
2588 }
2589 sleep(1);
2590 opened=true;
2591 }
2592 return flock;
2593 }
2594
2595 void
setIgnoreSize(const char * ignoresize_string)2596 AflockDevice::setIgnoreSize(const char* ignoresize_string)
2597 {
2598 if (!string2float(ignoreSize,ignoresize_string))
2599 swDebugf("argument %s is not a float, ignored\n",ignoresize_string);
2600 }
2601
2602
aflock(AflockDevice * device,char * receiver_string,bool headflag)2603 aflock::aflock(AflockDevice* device,char* receiver_string,bool headflag)
2604 : InputDevice("")
2605 {
2606 checkDevice = NULL;
2607 number_max_axis = 6;
2608 head=false;
2609 wand=false;
2610 headNavigation=false;
2611 aflockDevice=device;
2612
2613 error_message="";
2614
2615 if (headflag)
2616 {
2617 head=true;
2618 device_name=strdup("head");
2619 }
2620 else
2621 {
2622 wand=true;
2623 device_name=strdup("wand");
2624 }
2625
2626 if (!string2int(receiver,receiver_string))
2627 {
2628 error_message="";
2629 error_message+="argument ";
2630 error_message+=receiver_string;
2631 error_message+=" is not a integer";
2632 swDebugf(error_message);
2633 flock=NULL;
2634 return;
2635 }
2636
2637 if (device==NULL)
2638 {
2639 error_message="";
2640 error_message+="internal error, initialition failed";
2641 swDebugf(error_message);
2642 flock=NULL;
2643 return;
2644 }
2645
2646 flock=device->getAflock();
2647
2648 if (receiver==flock->getTransmitter())
2649 flock->setMasterUseReceiver();
2650
2651 number_buttons = 3;
2652 name=(char *)"Ascention flock of birds";
2653 maximum=1.0;
2654 aflock_factor=1.0;
2655 degreefactor=M_PI/180.0;
2656 aflockFirstFlag=true;
2657
2658 for (int i=0;i<7;i++)
2659 set_zero_on_release(i,false);
2660
2661 if (number_buttons>0)
2662 {
2663 button=new bool[number_buttons];
2664 for (int i=0;i<number_buttons;i++)
2665 button[i]=false;
2666 }
2667 # ifdef DEBUG
2668 swDebugf("InputDevice (%s) has %d axes and %d buttons. Driver ???\n",
2669 name, number_max_axis, number_buttons);
2670 # endif
2671 }
2672
~aflock(void)2673 aflock::~aflock(void)
2674 {
2675 }
2676
2677 void
guessInputDeviceNames(StringArray * names) const2678 aflock::guessInputDeviceNames(StringArray *names) const
2679 {
2680 names->resize(0);
2681 #ifdef SERIAL_DEVICE
2682 # ifdef OFFSET_SERIAL_DEVICE
2683 loopAddUnixDevices(names,SERIAL_DEVICE,OFFSET_SERIAL_DEVICE);
2684 # endif
2685 #endif
2686 loopAddUnixDevices(names,"/dev/ttyUSB");
2687 }
2688
prepareRead(void)2689 void aflock::prepareRead(void)
2690 {
2691 button_pressed=-1;
2692 button_released=-1;
2693
2694 if (flock==NULL)
2695 return;
2696 if (!flock->isActive())
2697 {
2698 swDebugf("Bird not active\n");
2699 return;
2700 }
2701 flock->prepareSample();
2702 }
2703
readInputDevice(void)2704 bool aflock::readInputDevice(void)
2705 {
2706 button_pressed=-1;
2707 button_released=-1;
2708
2709 if (flock==NULL)
2710 return false;
2711 if (!flock->isActive())
2712 {
2713 swDebugf("Bird not active\n");
2714 return false;
2715 }
2716 flock->sample();
2717 for (int i=0;i<number_buttons;i++)
2718 button[i]=false;
2719
2720 value[0]=-flock->yPos(receiver)/maxvalue(2)*aflock_factor;
2721 value[1]=-flock->zPos(receiver)/maxvalue(1)*aflock_factor;
2722 value[2]= flock->xPos(receiver)/maxvalue(0)*aflock_factor;
2723 float ignoreSize = aflockDevice->getIgnoreSize();
2724 if ((ignoreSize > 0) && !aflockFirstFlag)
2725 {
2726 #ifdef HAVE_AFLOCK_DEBUG
2727 #endif
2728 bool ret = true;
2729 if (fabs(old_value[0]-value[0])>ignoreSize)
2730 ret = false;
2731 if (fabs(old_value[1]-value[1])>ignoreSize)
2732 ret = false;
2733 if (fabs(old_value[2]-value[2])>ignoreSize)
2734 ret = false;
2735 if (ret == false)
2736 {
2737 swDebugf("ignoring jump flock %d: deltax=%f meter deltay=%f meter deltaz=%f meter\n",
2738 receiver,
2739 fabs(old_value[0]-value[0]),
2740 fabs(old_value[1]-value[1]),
2741 fabs(old_value[2]-value[2]));
2742 return false;
2743 }
2744 }
2745 old_value[0]=value[0];
2746 old_value[1]=value[1];
2747 old_value[2]=value[2];
2748 aflockFirstFlag=false;
2749 #ifdef HAVE_AFLOCK_DEBUG
2750 if (ignoreSize == 0)
2751 printf("flock %d: x=%f meter y=%f meter z=%f meter\n",receiver,
2752 value[0],value[1],value[2]);
2753 #endif
2754 value[3]=-flock->yRot(receiver)/maxvalue(3)*degreefactor;
2755 value[4]=-flock->zRot(receiver)/maxvalue(4)*degreefactor;
2756 value[5]=-flock->xRot(receiver)/maxvalue(5)*degreefactor;
2757
2758 return true;
2759 }
2760
isValid()2761 bool aflock::isValid()
2762 {
2763 if (aflockDevice)
2764 return aflockDevice->isValid();
2765 if (checkDevice)
2766 return checkUnixDevice(checkDevice);
2767 return false;
2768 }
2769
2770 # ifdef TEST_AFLOCK
main(int argc,char ** argv)2771 int main(int argc,char** argv)
2772 {
2773 AflockDevice* device=new AflockDevice();
2774 aflock device1(device,argv[1],true);
2775 for (int i=1;i<3000;i++)
2776 {
2777 if (device1.readInputDevice())
2778 printf("1 %g %g %g\n",device1.get_x(),device1.get_y(),device1.get_z());
2779 printf("1 rot %g %g %g\n",device1.get_xrot(),device1.get_yrot(),device1.get_zrot());
2780 }
2781 delete device;
2782 }
2783 # endif
2784 #endif
2785
2786 InputDevice *
createInputDevice(const char * device_option,const char * device_name)2787 InputDevice::createInputDevice(const char* device_option,
2788 const char* device_name)
2789 {
2790 InputDevice *ret = NULL;
2791 #ifdef LINUX_JOYSTICK
2792 if (strcmp(device_option,"-joystick")==0)
2793 ret=new linux_joystick(device_name);
2794 #endif
2795 #ifdef WINDOWS_JOYSTICK
2796 if (strcmp(device_option,"-joystick")==0)
2797 ret=new windows_joystick(device_name);
2798 #endif
2799 #ifdef HAVE_SDL_JOYSTICK
2800 if (strcmp(device_option,"-SDLjoystick")==0)
2801 ret=new SDL_joystick(device_name);
2802 #endif
2803 #ifdef HAVE_LIBSBALL
2804 if (strcmp(device_option,"-spaceball")==0)
2805 ret=new spaceball(device_name);
2806 #endif
2807 #ifdef HAVE_WINDOWS_SPACEBALL
2808 if (strcmp(device_option,"-spaceball")==0)
2809 ret=new windowsspaceball(device_name);
2810 #endif
2811 #ifdef HAVE_WINDOWS_P5
2812 if (strcmp(device_option,"-p5")==0)
2813 ret=new windowsp5(device_name);
2814 #endif
2815 #ifdef HAVE_NXT_DIALS
2816 if (strcmp(device_option,"-nxtdials")==0)
2817 ret=new nxtDials(device_name);
2818 #endif
2819 #ifdef HAVE_XINPUT
2820 if (strcmp(device_option,"-xinput")==0) {
2821 xinput *device=new xinput(device_name);
2822 if (device_name != NULL) {
2823 if (device == NULL)
2824 return NULL;
2825 if (device->get_number_axes() == 0)
2826 return NULL;
2827 }
2828 ret=device;
2829 }
2830 #endif
2831 #ifdef HAVE_AFLOCK
2832 // aflock devices must be handled differently
2833 if (strcmp(device_option,"-aflock")==0) {
2834 ret=new aflock(device_name);
2835 }
2836 #endif
2837 if (ret==NULL)
2838 swDebugf("internal program error: device option %s is missing here\n",
2839 device_option);
2840 return ret;
2841 }
2842
2843