void SkeletonModel::applyPalmData(int jointIndex, PalmData& palm) {
if (jointIndex == -1 || jointIndex >= _jointStates.size()) {
return;
}
const FBXGeometry& geometry = _geometry->getFBXGeometry();
float sign = (jointIndex == geometry.rightHandJointIndex) ? 1.0f : -1.0f;
int parentJointIndex = geometry.joints.at(jointIndex).parentIndex;
if (parentJointIndex == -1) {
return;
}
// rotate palm to align with its normal (normal points out of hand's palm)
glm::quat inverseRotation = glm::inverse(_rotation);
glm::vec3 palmPosition = inverseRotation * (palm.getPosition() - _translation);
glm::vec3 palmNormal = inverseRotation * palm.getNormal();
glm::vec3 fingerDirection = inverseRotation * palm.getFingerDirection();
glm::quat palmRotation = rotationBetween(geometry.palmDirection, palmNormal);
palmRotation = rotationBetween(palmRotation * glm::vec3(-sign, 0.0f, 0.0f), fingerDirection) * palmRotation;
if (Menu::getInstance()->isOptionChecked(MenuOption::AlternateIK)) {
setHandPosition(jointIndex, palmPosition, palmRotation);
} else if (Menu::getInstance()->isOptionChecked(MenuOption::AlignForearmsWithWrists)) {
float forearmLength = geometry.joints.at(jointIndex).distanceToParent * extractUniformScale(_scale);
glm::vec3 forearm = palmRotation * glm::vec3(sign * forearmLength, 0.0f, 0.0f);
setJointPosition(parentJointIndex, palmPosition + forearm,
glm::quat(), false, -1, false, glm::vec3(0.0f, -1.0f, 0.0f), PALM_PRIORITY);
JointState& parentState = _jointStates[parentJointIndex];
parentState.setRotationInBindFrame(palmRotation, PALM_PRIORITY);
// lock hand to forearm by slamming its rotation (in parent-frame) to identity
_jointStates[jointIndex].setRotationInConstrainedFrame(glm::quat(), PALM_PRIORITY);
} else {
inverseKinematics(jointIndex, palmPosition, palmRotation, PALM_PRIORITY);
}
}
void SkeletonModel::applyPalmData(int jointIndex, const QVector<int>& fingerJointIndices,
const QVector<int>& fingertipJointIndices, PalmData& palm) {
if (jointIndex == -1) {
return;
}
const FBXGeometry& geometry = _geometry->getFBXGeometry();
float sign = (jointIndex == geometry.rightHandJointIndex) ? 1.0f : -1.0f;
glm::quat palmRotation;
getJointRotation(jointIndex, palmRotation, true);
applyRotationDelta(jointIndex, rotationBetween(palmRotation * geometry.palmDirection, palm.getNormal()), false);
getJointRotation(jointIndex, palmRotation, true);
// sort the finger indices by raw x, get the average direction
QVector<IndexValue> fingerIndices;
glm::vec3 direction;
for (size_t i = 0; i < palm.getNumFingers(); i++) {
glm::vec3 fingerVector = palm.getFingers()[i].getTipPosition() - palm.getPosition();
float length = glm::length(fingerVector);
if (length > EPSILON) {
direction += fingerVector / length;
}
fingerVector = glm::inverse(palmRotation) * fingerVector * -sign;
IndexValue indexValue = { i, atan2f(fingerVector.z, fingerVector.x) };
fingerIndices.append(indexValue);
}
qSort(fingerIndices.begin(), fingerIndices.end());
// rotate palm according to average finger direction
float directionLength = glm::length(direction);
const int MIN_ROTATION_FINGERS = 3;
if (directionLength > EPSILON && palm.getNumFingers() >= MIN_ROTATION_FINGERS) {
applyRotationDelta(jointIndex, rotationBetween(palmRotation * glm::vec3(-sign, 0.0f, 0.0f), direction), false);
getJointRotation(jointIndex, palmRotation, true);
}
// no point in continuing if there are no fingers
if (palm.getNumFingers() == 0 || fingerJointIndices.isEmpty()) {
stretchArm(jointIndex, palm.getPosition());
return;
}
// match them up as best we can
float proportion = fingerIndices.size() / (float)fingerJointIndices.size();
for (int i = 0; i < fingerJointIndices.size(); i++) {
int fingerIndex = fingerIndices.at(roundf(i * proportion)).index;
glm::vec3 fingerVector = palm.getFingers()[fingerIndex].getTipPosition() -
palm.getFingers()[fingerIndex].getRootPosition();
int fingerJointIndex = fingerJointIndices.at(i);
int fingertipJointIndex = fingertipJointIndices.at(i);
glm::vec3 jointVector = extractTranslation(geometry.joints.at(fingertipJointIndex).bindTransform) -
extractTranslation(geometry.joints.at(fingerJointIndex).bindTransform);
setJointRotation(fingerJointIndex, rotationBetween(palmRotation * jointVector, fingerVector) * palmRotation, true);
}
stretchArm(jointIndex, palm.getPosition());
}
void SkeletonModel::applyPalmData(int jointIndex, const QVector<int>& fingerJointIndices,
const QVector<int>& fingertipJointIndices, PalmData& palm) {
if (jointIndex == -1) {
return;
}
const FBXGeometry& geometry = _geometry->getFBXGeometry();
float sign = (jointIndex == geometry.rightHandJointIndex) ? 1.0f : -1.0f;
int parentJointIndex = geometry.joints.at(jointIndex).parentIndex;
if (parentJointIndex == -1) {
return;
}
// rotate forearm to align with palm direction
glm::quat palmRotation;
getJointRotation(parentJointIndex, palmRotation, true);
applyRotationDelta(parentJointIndex, rotationBetween(palmRotation * geometry.palmDirection, palm.getNormal()), false);
getJointRotation(parentJointIndex, palmRotation, true);
// sort the finger indices by raw x, get the average direction
QVector<IndexValue> fingerIndices;
glm::vec3 direction;
for (size_t i = 0; i < palm.getNumFingers(); i++) {
glm::vec3 fingerVector = palm.getFingers()[i].getTipPosition() - palm.getPosition();
float length = glm::length(fingerVector);
if (length > EPSILON) {
direction += fingerVector / length;
}
fingerVector = glm::inverse(palmRotation) * fingerVector * -sign;
IndexValue indexValue = { (int)i, atan2f(fingerVector.z, fingerVector.x) };
fingerIndices.append(indexValue);
}
qSort(fingerIndices.begin(), fingerIndices.end());
// rotate forearm according to average finger direction
float directionLength = glm::length(direction);
const unsigned int MIN_ROTATION_FINGERS = 3;
if (directionLength > EPSILON && palm.getNumFingers() >= MIN_ROTATION_FINGERS) {
applyRotationDelta(parentJointIndex, rotationBetween(palmRotation * glm::vec3(-sign, 0.0f, 0.0f), direction), false);
getJointRotation(parentJointIndex, palmRotation, true);
}
// let wrist inherit forearm rotation
_jointStates[jointIndex].rotation = glm::quat();
// set elbow position from wrist position
glm::vec3 forearmVector = palmRotation * glm::vec3(sign, 0.0f, 0.0f);
setJointPosition(parentJointIndex, palm.getPosition() + forearmVector *
geometry.joints.at(jointIndex).distanceToParent * extractUniformScale(_scale));
}
void SkeletonModel::applyPalmData(int jointIndex, PalmData& palm) {
if (jointIndex == -1) {
return;
}
const FBXGeometry& geometry = _geometry->getFBXGeometry();
float sign = (jointIndex == geometry.rightHandJointIndex) ? 1.0f : -1.0f;
int parentJointIndex = geometry.joints.at(jointIndex).parentIndex;
if (parentJointIndex == -1) {
return;
}
// rotate palm to align with its normal (normal points out of hand's palm)
glm::quat palmRotation;
if (!Menu::getInstance()->isOptionChecked(MenuOption::AlternateIK) &&
Menu::getInstance()->isOptionChecked(MenuOption::AlignForearmsWithWrists)) {
getJointRotation(parentJointIndex, palmRotation, true);
} else {
getJointRotation(jointIndex, palmRotation, true);
}
palmRotation = rotationBetween(palmRotation * geometry.palmDirection, palm.getNormal()) * palmRotation;
// rotate palm to align with finger direction
glm::vec3 direction = palm.getFingerDirection();
palmRotation = rotationBetween(palmRotation * glm::vec3(-sign, 0.0f, 0.0f), direction) * palmRotation;
// set hand position, rotation
if (Menu::getInstance()->isOptionChecked(MenuOption::AlternateIK)) {
setHandPosition(jointIndex, palm.getPosition(), palmRotation);
} else if (Menu::getInstance()->isOptionChecked(MenuOption::AlignForearmsWithWrists)) {
glm::vec3 forearmVector = palmRotation * glm::vec3(sign, 0.0f, 0.0f);
setJointPosition(parentJointIndex, palm.getPosition() + forearmVector *
geometry.joints.at(jointIndex).distanceToParent * extractUniformScale(_scale));
setJointRotation(parentJointIndex, palmRotation, true);
_jointStates[jointIndex].rotation = glm::quat();
} else {
setJointPosition(jointIndex, palm.getPosition(), palmRotation, true);
}
}
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
}
static void setPalm(float deltaTime, int index) {
MyAvatar* avatar = Application::getInstance()->getAvatar();
Hand* hand = avatar->getHand();
PalmData* palm;
bool foundHand = false;
for (size_t j = 0; j < hand->getNumPalms(); j++) {
if (hand->getPalms()[j].getSixenseID() == 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(index);
}
palm->setActive(true);
// Read controller buttons and joystick into the hand
const QString PRIO_JOYSTICK_NAME = "PrioVR";
Joystick* prioJoystick = JoystickScriptingInterface::getInstance().joystickWithName(PRIO_JOYSTICK_NAME);
if (prioJoystick) {
const QVector<float> axes = prioJoystick->getAxes();
const QVector<bool> buttons = prioJoystick->getButtons();
if (axes.size() >= 4 && buttons.size() >= 4) {
if (index == LEFT_HAND_INDEX) {
palm->setControllerButtons(buttons[1] ? BUTTON_FWD : 0);
palm->setTrigger(buttons[0] ? 1.0f : 0.0f);
palm->setJoystick(axes[0], -axes[1]);
} else {
palm->setControllerButtons(buttons[3] ? BUTTON_FWD : 0);
palm->setTrigger(buttons[2] ? 1.0f : 0.0f);
palm->setJoystick(axes[2], -axes[3]);
}
}
}
// NOTE: this math is done in the worl-frame with unecessary complexity.
// TODO: transfom this to stay in the model-frame.
glm::vec3 position;
glm::quat rotation;
SkeletonModel* skeletonModel = &Application::getInstance()->getAvatar()->getSkeletonModel();
int jointIndex;
glm::quat inverseRotation = glm::inverse(Application::getInstance()->getAvatar()->getOrientation());
if (index == LEFT_HAND_INDEX) {
jointIndex = skeletonModel->getLeftHandJointIndex();
skeletonModel->getJointRotationInWorldFrame(jointIndex, rotation);
rotation = inverseRotation * rotation * glm::quat(glm::vec3(0.0f, PI_OVER_TWO, 0.0f));
} else {
jointIndex = skeletonModel->getRightHandJointIndex();
skeletonModel->getJointRotationInWorldFrame(jointIndex, rotation);
rotation = inverseRotation * rotation * glm::quat(glm::vec3(0.0f, -PI_OVER_TWO, 0.0f));
}
skeletonModel->getJointPositionInWorldFrame(jointIndex, position);
position = inverseRotation * (position - skeletonModel->getTranslation());
palm->setRawRotation(rotation);
// Compute current velocity from position change
glm::vec3 rawVelocity;
if (deltaTime > 0.0f) {
rawVelocity = (position - palm->getRawPosition()) / deltaTime;
} else {
rawVelocity = glm::vec3(0.0f);
}
palm->setRawVelocity(rawVelocity);
palm->setRawPosition(position);
// 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.0f) {
palm->setTipVelocity((newTipPosition - oldTipPosition) / deltaTime);
} else {
palm->setTipVelocity(glm::vec3(0.0f));
}
palm->setTipPosition(newTipPosition);
}
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
}
void SixenseManager::update(float deltaTime) {
#ifdef HAVE_SIXENSE
Hand* hand = DependencyManager::get<AvatarManager>()->getMyAvatar()->getHand();
if (_isInitialized && _isEnabled) {
#ifdef __APPLE__
SixenseBaseFunction sixenseGetNumActiveControllers =
(SixenseBaseFunction) _sixenseLibrary->resolve("sixenseGetNumActiveControllers");
#endif
if (sixenseGetNumActiveControllers() == 0) {
_hydrasConnected = false;
return;
}
PerformanceTimer perfTimer("sixense");
if (!_hydrasConnected) {
_hydrasConnected = true;
UserActivityLogger::getInstance().connectedDevice("spatial_controller", "hydra");
}
#ifdef __APPLE__
SixenseBaseFunction sixenseGetMaxControllers =
(SixenseBaseFunction) _sixenseLibrary->resolve("sixenseGetMaxControllers");
#endif
int maxControllers = sixenseGetMaxControllers();
// we only support two controllers
sixenseControllerData controllers[2];
#ifdef __APPLE__
SixenseTakeIntFunction sixenseIsControllerEnabled =
(SixenseTakeIntFunction) _sixenseLibrary->resolve("sixenseIsControllerEnabled");
SixenseTakeIntAndSixenseControllerData sixenseGetNewestData =
(SixenseTakeIntAndSixenseControllerData) _sixenseLibrary->resolve("sixenseGetNewestData");
#endif
int numControllersAtBase = 0;
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);
qCDebug(interfaceapp, "Found new Sixense controller, ID %i", data->controller_index);
}
// Disable the hands (and return to default pose) if both controllers are at base station
if (foundHand) {
palm->setActive(!_controllersAtBase);
} else {
palm->setActive(false); // if this isn't a Sixsense ID palm, always make it inactive
}
// 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) && !_controllersAtBase) {
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;
// Check to see if this hand/controller is on the base
const float CONTROLLER_AT_BASE_DISTANCE = 0.075f;
if (glm::length(position) < CONTROLLER_AT_BASE_DISTANCE) {
numControllersAtBase++;
}
// 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.0f;
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.0f, 1.0f, 0.0f)) * _orbRotation * rotation;
// Compute current velocity from position change
glm::vec3 rawVelocity;
if (deltaTime > 0.0f) {
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));
// Angular Velocity of Palm
glm::quat deltaRotation = rotation * glm::inverse(palm->getRawRotation());
glm::vec3 angularVelocity(0.0f);
float rotationAngle = glm::angle(deltaRotation);
if ((rotationAngle > EPSILON) && (deltaTime > 0.0f)) {
angularVelocity = glm::normalize(glm::axis(deltaRotation));
angularVelocity *= (rotationAngle / deltaTime);
palm->setRawAngularVelocity(angularVelocity);
} else {
palm->setRawAngularVelocity(glm::vec3(0.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);
}
// 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.0f) {
palm->setTipVelocity((newTipPosition - oldTipPosition) / deltaTime);
} else {
palm->setTipVelocity(glm::vec3(0.0f));
}
palm->setTipPosition(newTipPosition);
}
if (numActiveControllers == 2) {
updateCalibration(controllers);
}
_controllersAtBase = (numControllersAtBase == 2);
}
#endif // HAVE_SIXENSE
}
void BuckyBalls::grab(PalmData& palm, float deltaTime) {
float penetration;
glm::vec3 fingerTipPosition = palm.getFingerTipPosition();
if (palm.getControllerButtons() & BUTTON_FWD) {
if (!_bballIsGrabbed[palm.getSixenseID()]) {
// Look for a ball to grab
for (int i = 0; i < NUM_BBALLS; i++) {
glm::vec3 diff = _bballPosition[i] - fingerTipPosition;
penetration = glm::length(diff) - (_bballRadius[i] + COLLISION_RADIUS);
if (penetration < 0.f) {
_bballIsGrabbed[palm.getSixenseID()] = i;
}
}
}
if (_bballIsGrabbed[palm.getSixenseID()]) {
// If ball being grabbed, move with finger
glm::vec3 diff = _bballPosition[_bballIsGrabbed[palm.getSixenseID()]] - fingerTipPosition;
penetration = glm::length(diff) - (_bballRadius[_bballIsGrabbed[palm.getSixenseID()]] + COLLISION_RADIUS);
_bballPosition[_bballIsGrabbed[palm.getSixenseID()]] -= glm::normalize(diff) * penetration;
glm::vec3 fingerTipVelocity = palm.getTipVelocity();
if (_bballElement[_bballIsGrabbed[palm.getSixenseID()]] != 1) {
_bballVelocity[_bballIsGrabbed[palm.getSixenseID()]] = fingerTipVelocity;
}
_bballPosition[_bballIsGrabbed[palm.getSixenseID()]] = fingerTipPosition;
_bballColliding[_bballIsGrabbed[palm.getSixenseID()]] = 1.f;
}
} else {
_bballIsGrabbed[palm.getSixenseID()] = 0;
}
}