void CServerDriver_Hydra::ScanForNewControllers( bool bNotifyServer )
{
for ( int base = 0; base < sixenseGetMaxBases(); ++base )
{
if ( sixenseIsBaseConnected( base ) )
{
sixenseSetActiveBase( base );
for ( int i = 0; i < sixenseGetMaxControllers(); ++i )
{
if ( sixenseIsControllerEnabled( i ) )
{
char buf[256];
GenerateSerialNumber( buf, sizeof( buf ), base, i );
scope_lock lock( m_Mutex );
if ( !FindTrackedDeviceDriver( buf, vr::ITrackedDeviceServerDriver_Version ) )
{
DriverLog( "added new device %s\n", buf );
m_vecControllers.push_back( new CHydraHmdLatest( m_pDriverHost, base, i ) );
if ( bNotifyServer && m_pDriverHost )
{
m_pDriverHost->TrackedDeviceAdded( m_vecControllers.back()->GetSerialNumber() );
}
}
}
}
}
}
}
//===========================================================================
int cHydraDevice::open()
{
std::cout << "open";
// check if the system is available
if (!m_deviceAvailable) return (-1);
// if system is already opened then return
if (m_deviceReady) return (0);
/************************************************************************
STEP 3:
Here you need to implement code which open the connection to your
device. This may include opening a connection to an interface board
for instance or a USB port.
If the connection succeeds, set the variable 'result' to true.
otherwises, set the variable 'result' to false.
Verify that your device is calibrated. If your device
needs calibration then call method calibrate() for wich you will
provide code in STEP 5 further bellow.
*************************************************************************/
bool result = false;
// *** INSERT YOUR CODE HERE ***
// result = openConnectionToMyDevice();
std::cout << "Found " << sixenseGetMaxControllers() <<
" hydra controllers" << std::endl;
result = sixenseGetMaxControllers() > 0;
// update device status
if (result)
{
m_deviceReady = true;
return (0);
}
else
{
m_deviceReady = false;
return (-1);
}
}
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
}
bool FlyingMouse::TriggerCrossedThreshold(Hand h, float thresh) const
{
#ifdef USE_SIXENSE
const int maxControllers = sixenseGetMaxControllers();
for (int cont=0; cont<maxControllers; cont++)
{
if (triggerPassedThresholdOnController(g_lastAcd, g_curAcd, static_cast<char>(h), cont, thresh))
return true;
}
#endif // USE_SIXENSE
return false;
}
bool FlyingMouse::WasJustPressed(int buttonID) const
{
#ifdef USE_SIXENSE
const int maxControllers = sixenseGetMaxControllers();
for (int cont=0; cont<maxControllers; cont++)
{
if (buttonWasPressedOnController(g_lastAcd, g_curAcd, cont, buttonID))
return true;
}
#endif // USE_SIXENSE
return false;
}
float FlyingMouse::GetTriggerValue(Hand h) const
{
#ifdef USE_SIXENSE
const int maxControllers = sixenseGetMaxControllers();
for (int cont=0; cont<maxControllers; cont++)
{
const sixenseControllerData& da = g_curAcd.controllers[cont];
if (h == da.which_hand)
return da.trigger;
}
#endif
return 0.0f;
}
bool FlyingMouse::TriggerIsOverThreshold(Hand h, float thresh) const
{
#ifdef USE_SIXENSE
const int maxControllers = sixenseGetMaxControllers();
for (int cont=0; cont<maxControllers; cont++)
{
const sixenseControllerData& da = g_curAcd.controllers[cont];
if ((h == da.which_hand) &&
(da.trigger > thresh))
return true;
}
#endif
return false;
}
void HydraRenderer::Render(D3DRenderer* renderer)
{
CBPerObject perObject;
perObject.Material.HasDiffuseTex = false;
perObject.Material.HasNormalTex = false;
perObject.Material.HasSpecTex = false;
perObject.Material.HasEmissiveTex = false;
HydraManager* hydra = InputSystem::get()->getHydra();
for (int controller = 0; controller < sixenseGetMaxControllers(); controller++)
{
if (sixenseIsControllerEnabled(controller))
{
XMVECTOR rotationQuat = hydra->getRotation(controller);
XMVECTOR position = hydra->getPosition(controller);
//XMVectorSetZ(position, -XMVectorGetZ(position));//Flip Z axis
//position += XMVectorSet(0.0f, 0.9f, 0.0f, 0.0f);//Offset for table height
perObject.World = XMMatrixRotationQuaternion(XMQuaternionRotationAxis(XMLoadFloat3(&XMFLOAT3(1.0f, 0.0f, 0.0f)), XMConvertToRadians(90.0f))) *
XMMatrixTranslation(0.0f, 0.0f, 0.07f) *
//XMMatrixTranslation(0.0f, 0.0f, 0.25f) *
XMMatrixRotationQuaternion(rotationQuat) *
XMMatrixTranslationFromVector(position);
perObject.WorldInvTranspose = XMMatrixInverse(NULL, XMMatrixTranspose(perObject.World));
perObject.WorldViewProj = perObject.World * renderer->getPerFrameBuffer()->ViewProj;
renderer->setPerObjectBuffer(perObject);
mpPointerRenderer->Render(renderer);
//Render root trackers
perObject.World = XMMatrixRotationQuaternion(XMQuaternionRotationAxis(XMLoadFloat3(&XMFLOAT3(1.0f, 0.0f, 0.0f)), XMConvertToRadians(90.0f))) *
XMMatrixRotationQuaternion(rotationQuat) *
XMMatrixTranslationFromVector(position);
perObject.WorldInvTranspose = XMMatrixInverse(NULL, XMMatrixTranspose(perObject.World));
perObject.WorldViewProj = perObject.World * renderer->getPerFrameBuffer()->ViewProj;
renderer->setPerObjectBuffer(perObject);
mpRootRenderer->Render(renderer);
}
}
}
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
}
void SixenseManager::update(float deltaTime) {
#ifdef HAVE_SIXENSE
if (sixenseGetNumActiveControllers() == 0) {
return;
}
MyAvatar* avatar = Application::getInstance()->getAvatar();
Hand* hand = avatar->getHand();
int maxControllers = sixenseGetMaxControllers();
// we only support two controllers
sixenseControllerData controllers[2];
int numActiveControllers = 0;
for (int i = 0; i < maxControllers && numActiveControllers < 2; i++) {
if (!sixenseIsControllerEnabled(i)) {
continue;
}
sixenseControllerData* data = controllers + numActiveControllers;
++numActiveControllers;
sixenseGetNewestData(i, data);
// Set palm position and normal based on Hydra position/orientation
// Either find a palm matching the sixense controller, or make a new one
PalmData* palm;
bool foundHand = false;
for (size_t j = 0; j < hand->getNumPalms(); j++) {
if (hand->getPalms()[j].getSixenseID() == data->controller_index) {
palm = &(hand->getPalms()[j]);
foundHand = true;
}
}
if (!foundHand) {
PalmData newPalm(hand);
hand->getPalms().push_back(newPalm);
palm = &(hand->getPalms()[hand->getNumPalms() - 1]);
palm->setSixenseID(data->controller_index);
printf("Found new Sixense controller, ID %i\n", data->controller_index);
}
palm->setActive(true);
// Read controller buttons and joystick into the hand
palm->setControllerButtons(data->buttons);
palm->setTrigger(data->trigger);
palm->setJoystick(data->joystick_x, data->joystick_y);
glm::vec3 position(data->pos[0], data->pos[1], data->pos[2]);
// Transform the measured position into body frame.
glm::vec3 neck = _neckBase;
// Zeroing y component of the "neck" effectively raises the measured position a little bit.
neck.y = 0.f;
position = _orbRotation * (position - neck);
// Rotation of Palm
glm::quat rotation(data->rot_quat[3], -data->rot_quat[0], data->rot_quat[1], -data->rot_quat[2]);
rotation = glm::angleAxis(PI, glm::vec3(0.f, 1.f, 0.f)) * _orbRotation * rotation;
const glm::vec3 PALM_VECTOR(0.0f, -1.0f, 0.0f);
glm::vec3 newNormal = rotation * PALM_VECTOR;
palm->setRawNormal(newNormal);
palm->setRawRotation(rotation);
// Compute current velocity from position change
glm::vec3 rawVelocity = (position - palm->getRawPosition()) / deltaTime / 1000.f;
palm->setRawVelocity(rawVelocity); // meters/sec
palm->setRawPosition(position);
// use the velocity to determine whether there's any movement (if the hand isn't new)
const float MOVEMENT_SPEED_THRESHOLD = 0.05f;
if (glm::length(rawVelocity) > MOVEMENT_SPEED_THRESHOLD && foundHand) {
_lastMovement = usecTimestampNow();
}
// initialize the "finger" based on the direction
FingerData finger(palm, hand);
finger.setActive(true);
finger.setRawRootPosition(position);
const float FINGER_LENGTH = 300.0f; // Millimeters
const glm::vec3 FINGER_VECTOR(0.0f, 0.0f, FINGER_LENGTH);
const glm::vec3 newTipPosition = position + rotation * FINGER_VECTOR;
finger.setRawTipPosition(position + rotation * FINGER_VECTOR);
// Store the one fingertip in the palm structure so we can track velocity
glm::vec3 oldTipPosition = palm->getTipRawPosition();
palm->setTipVelocity((newTipPosition - oldTipPosition) / deltaTime / 1000.f);
palm->setTipPosition(newTipPosition);
// three fingers indicates to the skeleton that we have enough data to determine direction
palm->getFingers().clear();
palm->getFingers().push_back(finger);
palm->getFingers().push_back(finger);
palm->getFingers().push_back(finger);
}
if (numActiveControllers == 2) {
updateCalibration(controllers);
}
// if the controllers haven't been moved in a while, disable
const unsigned int MOVEMENT_DISABLE_DURATION = 30 * 1000 * 1000;
if (usecTimestampNow() - _lastMovement > MOVEMENT_DISABLE_DURATION) {
for (std::vector<PalmData>::iterator it = hand->getPalms().begin(); it != hand->getPalms().end(); it++) {
it->setActive(false);
}
}
#endif // HAVE_SIXENSE
}
void SixenseManager::update(float deltaTime) {
#ifdef HAVE_SIXENSE
// if the controllers haven't been moved in a while, disable
const unsigned int MOVEMENT_DISABLE_SECONDS = 3;
if (usecTimestampNow() - _lastMovement > (MOVEMENT_DISABLE_SECONDS * USECS_PER_SECOND)) {
Hand* hand = Application::getInstance()->getAvatar()->getHand();
for (std::vector<PalmData>::iterator it = hand->getPalms().begin(); it != hand->getPalms().end(); it++) {
it->setActive(false);
}
_lastMovement = usecTimestampNow();
}
if (sixenseGetNumActiveControllers() == 0) {
_hydrasConnected = false;
return;
}
PerformanceTimer perfTimer("sixense");
if (!_hydrasConnected) {
_hydrasConnected = true;
UserActivityLogger::getInstance().connectedDevice("spatial_controller", "hydra");
}
MyAvatar* avatar = Application::getInstance()->getAvatar();
Hand* hand = avatar->getHand();
int maxControllers = sixenseGetMaxControllers();
// we only support two controllers
sixenseControllerData controllers[2];
int numActiveControllers = 0;
for (int i = 0; i < maxControllers && numActiveControllers < 2; i++) {
if (!sixenseIsControllerEnabled(i)) {
continue;
}
sixenseControllerData* data = controllers + numActiveControllers;
++numActiveControllers;
sixenseGetNewestData(i, data);
// Set palm position and normal based on Hydra position/orientation
// Either find a palm matching the sixense controller, or make a new one
PalmData* palm;
bool foundHand = false;
for (size_t j = 0; j < hand->getNumPalms(); j++) {
if (hand->getPalms()[j].getSixenseID() == data->controller_index) {
palm = &(hand->getPalms()[j]);
foundHand = true;
}
}
if (!foundHand) {
PalmData newPalm(hand);
hand->getPalms().push_back(newPalm);
palm = &(hand->getPalms()[hand->getNumPalms() - 1]);
palm->setSixenseID(data->controller_index);
qDebug("Found new Sixense controller, ID %i", data->controller_index);
}
palm->setActive(true);
// Read controller buttons and joystick into the hand
palm->setControllerButtons(data->buttons);
palm->setTrigger(data->trigger);
palm->setJoystick(data->joystick_x, data->joystick_y);
// Emulate the mouse so we can use scripts
if (Menu::getInstance()->isOptionChecked(MenuOption::SixenseMouseInput)) {
emulateMouse(palm, numActiveControllers - 1);
}
// NOTE: Sixense API returns pos data in millimeters but we IMMEDIATELY convert to meters.
glm::vec3 position(data->pos[0], data->pos[1], data->pos[2]);
position *= METERS_PER_MILLIMETER;
// Transform the measured position into body frame.
glm::vec3 neck = _neckBase;
// Zeroing y component of the "neck" effectively raises the measured position a little bit.
neck.y = 0.f;
position = _orbRotation * (position - neck);
// Rotation of Palm
glm::quat rotation(data->rot_quat[3], -data->rot_quat[0], data->rot_quat[1], -data->rot_quat[2]);
rotation = glm::angleAxis(PI, glm::vec3(0.f, 1.f, 0.f)) * _orbRotation * rotation;
// Compute current velocity from position change
glm::vec3 rawVelocity;
if (deltaTime > 0.f) {
rawVelocity = (position - palm->getRawPosition()) / deltaTime;
} else {
rawVelocity = glm::vec3(0.0f);
}
palm->setRawVelocity(rawVelocity); // meters/sec
// adjustment for hydra controllers fit into hands
float sign = (i == 0) ? -1.0f : 1.0f;
rotation *= glm::angleAxis(sign * PI/4.0f, glm::vec3(0.0f, 0.0f, 1.0f));
if (_lowVelocityFilter) {
// Use a velocity sensitive filter to damp small motions and preserve large ones with
// no latency.
float velocityFilter = glm::clamp(1.0f - glm::length(rawVelocity), 0.0f, 1.0f);
position = palm->getRawPosition() * velocityFilter + position * (1.0f - velocityFilter);
rotation = safeMix(palm->getRawRotation(), rotation, 1.0f - velocityFilter);
palm->setRawPosition(position);
palm->setRawRotation(rotation);
} else {
palm->setRawPosition(position);
palm->setRawRotation(rotation);
}
// use the velocity to determine whether there's any movement (if the hand isn't new)
const float MOVEMENT_DISTANCE_THRESHOLD = 0.003f;
_amountMoved += rawVelocity * deltaTime;
if (glm::length(_amountMoved) > MOVEMENT_DISTANCE_THRESHOLD && foundHand) {
_lastMovement = usecTimestampNow();
_amountMoved = glm::vec3(0.0f);
}
// Store the one fingertip in the palm structure so we can track velocity
const float FINGER_LENGTH = 0.3f; // meters
const glm::vec3 FINGER_VECTOR(0.0f, 0.0f, FINGER_LENGTH);
const glm::vec3 newTipPosition = position + rotation * FINGER_VECTOR;
glm::vec3 oldTipPosition = palm->getTipRawPosition();
if (deltaTime > 0.f) {
palm->setTipVelocity((newTipPosition - oldTipPosition) / deltaTime);
} else {
palm->setTipVelocity(glm::vec3(0.f));
}
palm->setTipPosition(newTipPosition);
}
if (numActiveControllers == 2) {
updateCalibration(controllers);
}
#endif // HAVE_SIXENSE
}
