//===========================================================================
int cHydraDevice::getUserSwitch(int a_switchIndex, bool& a_status)
{
/************************************************************************
STEP 12:
Here you may implement code which reads the status of one or
more user switches on your device. An application may request to read the status
of a switch by passing its index number. The primary user switch mounted
on the stylus of a haptic device will receive the index number 0. The
second user switch is refered to as 1, and so on.
The return value of a switch (a_status) shall be equal to \b true if the button
is pressed or \b false otherwise.
If the operation fails return an error code such as -1 for instance.
*************************************************************************/
int error = 0;
// *** INSERT YOUR CODE HERE ***
sixenseAllControllerData acd;
sixenseGetAllNewestData( &acd );
if(a_switchIndex == 0)
a_status = acd.controllers[a_deviceNumber].buttons & 32;
if(a_switchIndex == 1)
a_status = acd.controllers[a_deviceNumber].buttons & 64;
if(a_switchIndex == 2)
a_status = acd.controllers[a_deviceNumber].buttons & 8;
if(a_switchIndex == 3)
a_status = acd.controllers[a_deviceNumber].buttons & 16;
return (error);
}
//===========================================================================
int cHydraDevice::getGripperAngleRad(double& a_angle)
{
/************************************************************************
STEP 8:
Here you may implement code which reads the position angle of your
gripper. The result must be returned in radian.
If the operation fails return an error code such as -1 for instance.
*************************************************************************/
int error = 0;
// *** INSERT YOUR CODE HERE, MODIFY CODE BELLOW ACCORDINGLY ***
// return gripper angle
sixenseAllControllerData acd;
sixenseGetAllNewestData( &acd );
a_angle = acd.controllers[a_deviceNumber].trigger * 3.141592 / 4;
// std::cout << "hydragrip " << a_angle << std::endl;
// estimate gripper velocity
estimateGripperVelocity(a_angle);
// exit
return (error);
}
void HydraManager::Update(float dt)
{
int left_index = sixenseUtils::getTheControllerManager()->getIndex( sixenseUtils::ControllerManager::P1L );
int right_index = sixenseUtils::getTheControllerManager()->getIndex( sixenseUtils::ControllerManager::P1R );
sixenseSetActiveBase(0);
sixenseGetAllNewestData(&mAcd);
}
void FlyingMouse::updateHydraData()
{
#ifdef USE_SIXENSE
//int left_index = sixenseUtils::getTheControllerManager()->getIndex(sixenseUtils::ControllerManager::P1L);
//int right_index = sixenseUtils::getTheControllerManager()->getIndex(sixenseUtils::ControllerManager::P1R);
m_active = false;
const int maxBases = sixenseGetMaxBases();
for (int base = 0; base < maxBases; ++base)
{
sixenseSetActiveBase(base);
if (!sixenseIsBaseConnected(base))
continue;
sixenseAllControllerData acd;
sixenseGetAllNewestData(&acd);
const int maxControllers = sixenseGetMaxControllers();
for (int cont = 0; cont < maxControllers; cont++)
{
if (!sixenseIsControllerEnabled(cont))
continue;
m_active = true;
const sixenseControllerData& cd = acd.controllers[cont];
float* mtx = mtxL;
if (cd.which_hand == 2)
mtx = mtxR;
mtx[0] = cd.rot_mat[0][0];
mtx[1] = cd.rot_mat[0][1];
mtx[2] = cd.rot_mat[0][2];
mtx[3] = 0.0f;
mtx[4] = cd.rot_mat[1][0];
mtx[5] = cd.rot_mat[1][1];
mtx[6] = cd.rot_mat[1][2];
mtx[7] = 0.0f;
mtx[ 8] = cd.rot_mat[2][0];
mtx[ 9] = cd.rot_mat[2][1];
mtx[10] = cd.rot_mat[2][2];
mtx[11] = 0.0f;
const float posS = 0.001f; // Try to match world space
mtx[12] = cd.pos[0] * posS;
mtx[13] = cd.pos[1] * posS;
mtx[14] = cd.pos[2] * posS;
mtx[15] = 1.0f;
}
g_lastAcd = g_curAcd;
g_curAcd = acd;
}
//sixenseUtils::getTheControllerManager()->update(&acd);
#endif // USE_SIXENSE
}
//===========================================================================
int cHydraDevice::getPosition(cVector3d& a_position)
{
//std::cout << "get pos";
/************************************************************************
STEP 7:
Here you may implement code which reads the position (X,Y,Z) from
your haptic device. Read the values from your device and modify
the local variable (x,y,z) accordingly.
If the operation fails return an error code such as -1 for instance.
Note:
For consistency, units must be in meters.
If your device is located in front of you, the x-axis is pointing
towards you (the operator). The y-axis points towards your right
hand side and the z-axis points up towards the sky.
*************************************************************************/
int error = 0;
double x,y,z;
// *** INSERT YOUR CODE HERE, MODIFY CODE BELLOW ACCORDINGLY ***
sixenseAllControllerData acd;
sixenseGetAllNewestData( &acd );
y = acd.controllers[a_deviceNumber].pos[0] / 1000.0f;
z = acd.controllers[a_deviceNumber].pos[1] / 1000.0f - 0.3;
x = acd.controllers[a_deviceNumber].pos[2] / 1000.0f - 0.3;
// offset
cMatrix3d r;
getRotation(r);
cVector3d v(-0.08,0,0);
v = r * v;
// store new position values
a_position.set(x+v.x(), y+v.y(), z+v.z());
// estimate linear velocity
estimateLinearVelocity(a_position);
// exit
return (error);
}
bool Hydra_Tracker::GiveHeadPoseData(double *data)
{
sixenseSetActiveBase(0);
sixenseAllControllerData acd;
sixenseGetAllNewestData( &acd );
//sixenseUtils::getTheControllerManager()->update( &acd );
//sixenseControllerData cd;
//Rotation quat = Rotation(acd.controllers[0].rot_quat[1],acd.controllers[0].rot_quat[2],acd.controllers[0].rot_quat[3],acd.controllers[0].rot_quat[0]);
sixenseMath::Matrix4 mat = sixenseMath::Matrix4(acd.controllers[0].rot_mat);// sixenseMath::Quat(acd.controllers[0].rot_quat[1],acd.controllers[0].rot_quat[2],acd.controllers[0].rot_quat[3],acd.controllers[0].rot_quat[0]);
double yaw = 0.0f;
double pitch = 0.0f;
double roll = 0.0f;
float ypr[3];
mat.getEulerAngles().fill(ypr);
newHeadPose[Yaw] = ypr[0];
newHeadPose[Pitch] = ypr[1];
newHeadPose[Roll] = ypr[2];
newHeadPose[TX] = acd.controllers[0].pos[0]/50.0f;
newHeadPose[TY] = acd.controllers[0].pos[1]/50.0f;
newHeadPose[TZ] = acd.controllers[0].pos[2]/50.0f;
if (bEnableX) {
data[TX] = newHeadPose[TX];
}
if (bEnableY) {
data[TY] = newHeadPose[TY];
}
if (bEnableY) {
data[TZ] = newHeadPose[TZ];
}
if (bEnableYaw) {
data[Yaw] = newHeadPose[Yaw] * 57.295781f;
}
if (bEnablePitch) {
data[Pitch] = newHeadPose[Pitch] * 57.295781f;
}
if (bEnableRoll) {
data[Roll] = newHeadPose[Roll] * 57.295781f;
}
return true;
}
void Hydra_Tracker::GetHeadPoseData(double *data)
{
sixenseSetActiveBase(0);
sixenseAllControllerData acd;
sixenseGetAllNewestData( &acd );
sixenseMath::Matrix4 mat = sixenseMath::Matrix4(acd.controllers[0].rot_mat);
float ypr[3];
mat.getEulerAngles().fill(ypr);
newHeadPose[Yaw] = ypr[0];
newHeadPose[Pitch] = ypr[1];
newHeadPose[Roll] = ypr[2];
newHeadPose[TX] = acd.controllers[0].pos[0]/50.0f;
newHeadPose[TY] = acd.controllers[0].pos[1]/50.0f;
newHeadPose[TZ] = acd.controllers[0].pos[2]/50.0f;
if (s.bEnableX) {
data[TX] = newHeadPose[TX];
}
if (s.bEnableY) {
data[TY] = newHeadPose[TY];
}
if (s.bEnableY) {
data[TZ] = newHeadPose[TZ];
}
if (s.bEnableYaw) {
data[Yaw] = newHeadPose[Yaw] * 57.295781f;
}
if (s.bEnablePitch) {
data[Pitch] = newHeadPose[Pitch] * 57.295781f;
}
if (s.bEnableRoll) {
data[Roll] = newHeadPose[Roll] * 57.295781f;
}
}
void CServerDriver_Hydra::ThreadFunc()
{
// We know the sixense SDK thread is running at "60 FPS", but we don't know when
// those frames are. To minimize latency, we sleep for slightly less than the
// target rate, and detect when the frame has not advanced to wait a bit longer.
auto longInterval = std::chrono::milliseconds( 16 );
auto retryInterval = std::chrono::milliseconds( 2 );
auto scanInterval = std::chrono::seconds( 1 );
auto pollDeadline = std::chrono::steady_clock::now();
auto scanDeadline = std::chrono::steady_clock::now() + scanInterval;
#ifdef _WIN32
// Request at least 2ms timing granularity for the life of this process
timeBeginPeriod( 2 );
#endif
while ( !m_bStopRequested )
{
// Check for new controllers here because sixense API is modal
// (e.g. sixenseSetActiveBase()) so it can't happen in parallel with pose updates
if ( pollDeadline > scanDeadline )
{
ScanForNewControllers( true );
scanDeadline += scanInterval;
}
bool bAnyActivated = false;
bool bAllUpdated = true;
for ( int base = 0; base < sixenseGetMaxBases(); ++base )
{
if ( !sixenseIsBaseConnected( base ) )
continue;
sixenseAllControllerData acd;
sixenseSetActiveBase( base );
if ( sixenseGetAllNewestData( &acd ) != SIXENSE_SUCCESS )
continue;
for ( int id = 0; id < sixenseGetMaxControllers(); ++id )
{
for ( auto it = m_vecControllers.begin(); it != m_vecControllers.end(); ++it )
{
CHydraHmdLatest *pHydra = *it;
if ( pHydra->IsActivated() && pHydra->HasControllerId( base, id ) )
{
bAnyActivated = true;
// Returns true if this is new data (so we can sleep for long interval)
if ( !pHydra->Update( acd.controllers[id] ) )
{
bAllUpdated = false;
}
break;
}
}
}
}
CheckForChordedSystemButtons();
// If everyone just got new data, we can wait about 1/60s, else try again soon
pollDeadline += !bAnyActivated ? scanInterval :
bAllUpdated ? longInterval : retryInterval;
std::this_thread::sleep_until( pollDeadline );
}
#ifdef _WIN32
timeEndPeriod( 2 );
#endif
}
int main(int argc, char **argv)
{
ArgsParser arg(argc, argv);
// get help
if (argc < 2
|| 0 == strcasecmp(argv[1], "-h")
|| 0 == strcasecmp(argv[1], "--help"))
{
displayHelp(argv[0]);
exit(-1);
}
const char *target = arg.getOpt("-t", "--target", "localhost:7777");
const char *rateStr = arg.getOpt("-r", "--rate", "20");
const char *sendRate = arg.getOpt("-s", "--sendrate", "16");
// get port on which the dtrack sends the data
float rate = atof(rateStr);
// verbose max. once per second
int sendsPerVerbose;
if (rate < 1)
sendsPerVerbose = 1;
else
sendsPerVerbose = (int)rate;
// select() delay record
rate = 1.0 / rate;
struct timeval delay;
delay.tv_sec = (int)rate;
delay.tv_usec = (int)(1e6 * (rate - delay.tv_sec));
// +++++++++++++++ prepare mapping +++++++++++++++
int artID[MAXSENSORS], coverID[MAXSENSORS]; // COVER IDs for sensor[i]
if (arg.numArgs() > MAXSENSORS)
{
std::cerr << "Only " << MAXSENSORS << " sensors allowed" << std::endl;
}
int i;
int numSensors = 0;
for (i = 0; i < arg.numArgs(); i++)
{
coverID[numSensors] = i;
bool ok = splitOpt(arg[i], artID[numSensors], coverID[numSensors]);
if (ok)
{
numSensors++;
}
else
{
std::cerr << "Illegal device selection: " << arg[i] << std::endl;
exit(0);
}
}
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Show what we will do
printf("\n");
printf(" +-----------------------------------------------------+\n");
printf(" + VRC HydraServer %-10s (C) 2005 HLRS +\n", HydraServerVersion);
printf(" +-----------------------------------------------------+\n");
printf(" + Settings: +\n");
printf(" + UDP Target: %-30s +\n", target);
printf(" + Send Rate: %-3.1f Packets/s +\n", 1.0 / rate);
printf(" +-----------------------------------------------------+\n");
printf(" + Mapping: +\n");
for (i = 0; i < numSensors; i++)
{
printf(" + Target %c%-2d --> COVER ID %-2d +\n",
((artID[i] >= 10) ? 'F' : 'B'),
(artID[i] % 10) + 1,
coverID[i]);
}
printf(" +-----------------------------------------------------+\n\n");
/// ++++++++++++++++++++++++++ All parameters set - start work +++++++++++++++++++
signal(SIGINT, sigHandler);
#ifndef WIN32
signal(SIGPIPE, sigHandler);
signal(SIGCHLD, sigHandler);
#endif
signal(SIGTERM, sigHandler);
// create udp socket
UDP_Sender sender(target);
if (sender.isBad())
{
std::cerr << "Could not start UDP server to "
<< target << " : "
<< sender.errorMessage() << std::endl;
return -1;
}
int init = sixenseInit();
int activebase = sixenseSetActiveBase(0);
int basecolor = sixenseSetBaseColor(255, 0, 0);
sixenseAllControllerData acd;
int reshigh = sixenseSetHighPriorityBindingEnabled(1);
int autoenable = sixenseAutoEnableHemisphereTracking(1);
int frame = 0;
while (1)
{
sixenseGetAllNewestData(&acd);
Sleep((int)*sendRate);
/*fprintf(stderr,"left pos: x = %f\ty = %f\tz = %f\nright pos: x = %f\ty = %f\t z = %f\n\n",
acd.controllers[0].pos[0],acd.controllers[0].pos[1],acd.controllers[0].pos[2],
acd.controllers[1].pos[0],acd.controllers[1].pos[1],acd.controllers[1].pos[2]); */
for (i = 0; i < numSensors; i++)
{
char sendbuffer[2048];
sprintf(sendbuffer, "VRC %d %3d [%5.1f %5.1f %5.1f] - [%6.3f %6.3f %6.3f %6.3f %6.3f %6.3f %6.3f %6.3f %6.3f] - [ %d %d %s]",
coverID[i], acd.controllers[i].buttons, acd.controllers[i].pos[0], acd.controllers[i].pos[1], acd.controllers[i].pos[2],
acd.controllers[i].rot_mat[0][0], acd.controllers[i].rot_mat[0][1], acd.controllers[i].rot_mat[0][2],
acd.controllers[i].rot_mat[1][0], acd.controllers[i].rot_mat[1][1], acd.controllers[i].rot_mat[1][2],
acd.controllers[i].rot_mat[2][0], acd.controllers[i].rot_mat[2][1], acd.controllers[i].rot_mat[2][2],
acd.controllers[i].joystick_x, acd.controllers[i].joystick_y, compatibilityString);
sender.send(sendbuffer);
if (frame % sendsPerVerbose == 0)
{
fprintf(stderr, "%s\n", sendbuffer);
}
}
if (frame % sendsPerVerbose == 0)
{
fprintf(stderr, "---\n");
}
frame++;
}
sixenseExit();
return 0;
}
//===========================================================================
int cHydraDevice::getRotation(cMatrix3d& a_rotation)
{
/************************************************************************
STEP 7:
Here you may implement code which reads the orientation frame from
your haptic device. The orientation frame is expressed by a 3x3
rotation matrix. The 1st column of this matrix corresponds to the
x-axis, the 2nd column to the y-axis and the 3rd column to the z-axis.
The length of each column vector should be of length 1 and vectors need
to be perpendicular to each other.
If the operation fails return an error code such as -1 for instance.
Note:
If your device is located in front of you, the x-axis is pointing
towards you (the operator). The y-axis points towards your right
hand side and the z-axis points up towards the sky.
If your device has a stylus, make sure that you set the reference frame
so that the x-axis corresponds to the axis of the stylus.
*************************************************************************/
int error = 0;
double r00, r01, r02, r10, r11, r12, r20, r21, r22;
cMatrix3d frame;
frame.identity();
// *** INSERT YOUR CODE HERE, MODIFY CODE BELLOW ACCORDINGLY ***
// if the device does not provide any rotation capabilities
// we set the rotation matrix equal to the identiy matrix.
sixenseAllControllerData acd;
sixenseGetAllNewestData( &acd );
// sixense is COLUMN MAJOR!
r00 = acd.controllers[a_deviceNumber].rot_mat[0][0];
r01 = acd.controllers[a_deviceNumber].rot_mat[1][0];
r02 = acd.controllers[a_deviceNumber].rot_mat[2][0];
r10 = acd.controllers[a_deviceNumber].rot_mat[0][1];
r11 = acd.controllers[a_deviceNumber].rot_mat[1][1];
r12 = acd.controllers[a_deviceNumber].rot_mat[2][1];
r20 = acd.controllers[a_deviceNumber].rot_mat[0][2];
r21 = acd.controllers[a_deviceNumber].rot_mat[1][2];
r22 = acd.controllers[a_deviceNumber].rot_mat[2][2];
frame.set(r00, r01, r02,
r10, r11, r12,
r20, r21, r22);
cMatrix3d permute_rows(0, 0, 1,
1, 0, 0,
0, 1, 0);
cMatrix3d permute_columns(0, 1, 0,
0, 0, 1,
1, 0, 0);
a_rotation = permute_rows*frame*permute_columns;
// estimate angular velocity
estimateAngularVelocity(a_rotation);
// exit
return (error);
}
void updateSixenseControllerData() {
FIXME!! this needs error handling, big style.
sixenseSetActiveBase(base);
sixenseGetAllNewestData(&acd);
}
void dev::sixense::translate()
{
sixenseAllControllerData curr;
if (status && sixenseGetAllNewestData(&curr) == SIXENSE_SUCCESS)
{
sixenseControllerData *a = curr.controllers + hand_controller;
sixenseControllerData *b = prev.controllers + hand_controller;
app::event E;
// Generate motion events.
if (memcmp(a->pos, b->pos, 3 * sizeof (float)) ||
memcmp(a->rot_quat, b->rot_quat, 4 * sizeof (float)))
{
double p[3];
double q[4];
p[0] = a->pos[0] / 1000.0;
p[1] = a->pos[1] / 1000.0;
p[2] = a->pos[2] / 1000.0;
q[0] = a->rot_quat[0];
q[1] = a->rot_quat[1];
q[2] = a->rot_quat[2];
q[3] = a->rot_quat[3];
::host->process_event(E.mk_point(1, p, q));
}
// Generate joystick events.
if (a->joystick_x != b->joystick_x)
::host->process_event(E.mk_axis(1, 0, a->joystick_x));
if (a->joystick_y != b->joystick_y)
::host->process_event(E.mk_axis(1, 1, a->joystick_y));
if (a->trigger != b->trigger)
::host->process_event(E.mk_axis(1, 2, a->trigger));
// Generate button events.
if ((a->buttons ^ b->buttons) & SIXENSE_BUTTON_START)
::host->process_event(E.mk_button(1, 0, a->buttons & SIXENSE_BUTTON_START));
if ((a->buttons ^ b->buttons) & SIXENSE_BUTTON_BUMPER)
::host->process_event(E.mk_button(1, 1, a->buttons & SIXENSE_BUTTON_BUMPER));
if ((a->buttons ^ b->buttons) & SIXENSE_BUTTON_JOYSTICK)
::host->process_event(E.mk_button(1, 2, a->buttons & SIXENSE_BUTTON_JOYSTICK));
// Generate click events.
if ((a->buttons ^ b->buttons) & SIXENSE_BUTTON_1)
::host->process_event(E.mk_click(1, 0, a->buttons & SIXENSE_BUTTON_1));
if ((a->buttons ^ b->buttons) & SIXENSE_BUTTON_2)
::host->process_event(E.mk_click(2, 0, a->buttons & SIXENSE_BUTTON_2));
if ((a->buttons ^ b->buttons) & SIXENSE_BUTTON_3)
::host->process_event(E.mk_click(3, 0, a->buttons & SIXENSE_BUTTON_3));
if ((a->buttons ^ b->buttons) & SIXENSE_BUTTON_4)
::host->process_event(E.mk_click(4, 0, a->buttons & SIXENSE_BUTTON_4));
prev = curr;
}
}
