float HydraManager::getTrigger(int controllerIndex) const
{
if (sixenseIsControllerEnabled(controllerIndex))
return mAcd.controllers[controllerIndex].trigger;
else
return 0.0f;
}
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() );
}
}
}
}
}
}
}
unsigned int HydraManager::getButtons(int controllerIndex) const
{
if (controllerIndex < 2 && sixenseIsControllerEnabled(controllerIndex))
return mAcd.controllers[controllerIndex].buttons;
else
return 0;
}
bool HydraManager::isDocked(int controllerIndex) const
{
if (sixenseIsControllerEnabled(controllerIndex) && mAcd.controllers[controllerIndex].is_docked != 1)
return false;
return true;
}
XMVECTOR HydraManager::getPosition(int controllerIndex) const
{
if (sixenseIsControllerEnabled(controllerIndex))
return XMLoadFloat3(&XMFLOAT3(-mAcd.controllers[controllerIndex].pos[0],
mAcd.controllers[controllerIndex].pos[1] + 900.0f, //Table height offset in mm
-mAcd.controllers[controllerIndex].pos[2])) / 1000.0f;
return XMVectorZero();
}
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
}
XMFLOAT2 HydraManager::getJoystick(int controllerIndex) const
{
XMFLOAT2 directionVector(0.0f, 0.0f);
if (sixenseIsControllerEnabled(controllerIndex))
{
XMStoreFloat2(&directionVector, XMVector2Normalize(XMVectorSet(mAcd.controllers[controllerIndex].joystick_x,
mAcd.controllers[controllerIndex].joystick_y,
0.0f, 0.0f)));
}
return directionVector;
}
XMVECTOR HydraManager::getRotation(int controllerIndex) const
{
if (sixenseIsControllerEnabled(controllerIndex))
{
XMVECTOR axis;
float angle;
XMQuaternionToAxisAngle(&axis, &angle, XMLoadFloat4(&XMFLOAT4(&mAcd.controllers[controllerIndex].rot_quat[0])));
axis = XMVectorSet(-XMVectorGetX(axis), XMVectorGetY(axis), -XMVectorGetZ(axis), 0.0f);
XMVECTOR rotationQuat = XMQuaternionRotationAxis(axis, angle);
return rotationQuat;
}
return XMQuaternionIdentity();
}
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 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
}
