410 std::cerr <<
"vrpn_Tracker_DeadReckoning_Rotation::test: Out of memory" << std::endl;
419 q_vec_set(poseResponse.pos, 1, 1, 1);
420 q_make(poseResponse.quat, 0, 0, 1, 0);
421 poseResponse.time.tv_sec = 0;
422 poseResponse.time.tv_usec = 0;
423 poseResponse.sensor = -1;
429 q_vec_type
pos = { 1, 1, 1 };
430 q_type quat = { 0, 0, 0, 1 };
431 struct timeval firstSent;
441 if ( (poseResponse.time.tv_sec != firstSent.tv_sec + 1)
442 || (poseResponse.sensor != 1)
443 || (q_vec_distance(poseResponse.pos,
pos) > 1e-10)
444 || (poseResponse.quat[Q_W] != 1)
447 std::cerr <<
"vrpn_Tracker_DeadReckoning_Rotation::test(): Got unexpected"
448 <<
" initial response: pos (" << poseResponse.pos[Q_X] <<
", "
449 << poseResponse.pos[Q_Y] <<
", " << poseResponse.pos[Q_Z] <<
"), quat ("
450 << poseResponse.quat[Q_X] <<
", " << poseResponse.quat[Q_Y] <<
", "
451 << poseResponse.quat[Q_Z] <<
", " << poseResponse.quat[Q_W] <<
")"
452 <<
" from sensor " << poseResponse.sensor
453 <<
" at time " << poseResponse.time.tv_sec <<
":" << poseResponse.time.tv_usec
460 std::cerr <<
"vrpn_Tracker_DeadReckoning_Rotation::test(): delete failed" << std::endl;
471 q_vec_type pos2 = { 2, 1, 1 };
473 double angle2 = 0.4 * 90 *
VRPN_PI / 180.0;
474 q_from_axis_angle(quat2, 0, 0, 1, angle2);
475 struct timeval p4Second = { 0, 400000 };
484 double x, y, z, angle;
485 q_to_axis_angle(&x, &y, &z, &angle, poseResponse.quat);
486 if ((poseResponse.time.tv_sec != firstPlusP4.tv_sec + 1)
487 || (poseResponse.sensor != 0)
488 || (q_vec_distance(poseResponse.pos, pos2) > 1e-10)
489 || !isClose(x, 0) || !isClose(y, 0) || !isClose(z, 1)
490 || !isClose(angle, 1.4 * 90 *
VRPN_PI / 180.0)
493 std::cerr <<
"vrpn_Tracker_DeadReckoning_Rotation::test(): Got unexpected"
494 <<
" predicted pose response: pos (" << poseResponse.pos[Q_X] <<
", "
495 << poseResponse.pos[Q_Y] <<
", " << poseResponse.pos[Q_Z] <<
"), quat ("
496 << poseResponse.quat[Q_X] <<
", " << poseResponse.quat[Q_Y] <<
", "
497 << poseResponse.quat[Q_Z] <<
", " << poseResponse.quat[Q_W] <<
")"
498 <<
" from sensor " << poseResponse.sensor
505 std::cerr <<
"vrpn_Tracker_DeadReckoning_Rotation::test(): delete failed" << std::endl;
517 q_vec_type
vel = { 0, 0, 0 };
525 q_to_axis_angle(&x, &y, &z, &angle, poseResponse.quat);
526 if ((poseResponse.time.tv_sec != firstSent.tv_sec + 1)
527 || (poseResponse.sensor != 1)
528 || (q_vec_distance(poseResponse.pos,
pos) > 1e-10)
529 || !isClose(x, 0) || !isClose(y, 0) || !isClose(z, 1)
530 || !isClose(angle, 90 *
VRPN_PI / 180.0)
533 std::cerr <<
"vrpn_Tracker_DeadReckoning_Rotation::test(): Got unexpected"
534 <<
" predicted velocity response: pos (" << poseResponse.pos[Q_X] <<
", "
535 << poseResponse.pos[Q_Y] <<
", " << poseResponse.pos[Q_Z] <<
"), quat ("
536 << poseResponse.quat[Q_X] <<
", " << poseResponse.quat[Q_Y] <<
", "
537 << poseResponse.quat[Q_Z] <<
", " << poseResponse.quat[Q_W] <<
")"
538 <<
" from sensor " << poseResponse.sensor
545 std::cerr <<
"vrpn_Tracker_DeadReckoning_Rotation::test(): delete failed" << std::endl;
558 struct timeval oneSecond = { 1, 0 };
561 q_from_axis_angle(quat3, 1, 0, 0,
VRPN_PI);
564 q_from_axis_angle(quat4, 0, 0, 1,
VRPN_PI);
572 q_to_axis_angle(&x, &y, &z, &angle, poseResponse.quat);
573 if ((poseResponse.time.tv_sec != firstPlusOne.tv_sec + 1)
574 || (poseResponse.sensor != 1)
575 || (q_vec_distance(poseResponse.pos,
pos) > 1e-10)
576 || !isClose(x, 0) || !isClose(fabs(y), 1) || !isClose(z, 0)
577 || !isClose(fabs(angle),
VRPN_PI)
580 std::cerr <<
"vrpn_Tracker_DeadReckoning_Rotation::test(): Got unexpected"
581 <<
" predicted pose + velocity response: pos (" << poseResponse.pos[Q_X] <<
", "
582 << poseResponse.pos[Q_Y] <<
", " << poseResponse.pos[Q_Z] <<
"), quat ("
583 << poseResponse.quat[Q_X] <<
", " << poseResponse.quat[Q_Y] <<
", "
584 << poseResponse.quat[Q_Z] <<
", " << poseResponse.quat[Q_W] <<
")"
585 <<
" from sensor " << poseResponse.sensor
592 std::cerr <<
"vrpn_Tracker_DeadReckoning_Rotation::test(): delete failed" << std::endl;
609 struct timeval firstPlusTwo =
vrpn_TimevalSum(firstPlusOne, oneSecond);
610 q_from_axis_angle(quat3, 1, 0, 0,
VRPN_PI);
613 q_from_axis_angle(quat4, 0, 0, 1,
VRPN_PI / 2);
615 q_mult(quat5, quat4, quat3);
623 q_to_axis_angle(&x, &y, &z, &angle, poseResponse.quat);
624 if ((poseResponse.time.tv_sec != firstPlusTwo.tv_sec + 1)
625 || (poseResponse.sensor != 0)
626 || (q_vec_distance(poseResponse.pos,
pos) > 1e-10)
627 || !isClose(x, 0) || !isClose(fabs(y), 1.0) || !isClose(z, 0)
628 || !isClose(fabs(angle),
VRPN_PI)
631 std::cerr <<
"vrpn_Tracker_DeadReckoning_Rotation::test(): Got unexpected"
632 <<
" predicted pose + pose response: pos (" << poseResponse.pos[Q_X] <<
", "
633 << poseResponse.pos[Q_Y] <<
", " << poseResponse.pos[Q_Z] <<
"), quat ("
634 << poseResponse.quat[Q_X] <<
", " << poseResponse.quat[Q_Y] <<
", "
635 << poseResponse.quat[Q_Z] <<
", " << poseResponse.quat[Q_W] <<
")"
636 <<
" from sensor " << poseResponse.sensor
637 <<
"; axis = (" << x <<
", " << y <<
", " << z <<
"), angle = "
645 std::cerr <<
"vrpn_Tracker_DeadReckoning_Rotation::test(): delete failed" << std::endl;
659 std::cerr <<
"vrpn_Tracker_DeadReckoning_Rotation::test(): delete failed" << std::endl;