void LeapMotionPlugin::InputDevice::update(float deltaTime, const controller::InputCalibrationData& inputCalibrationData,
const std::vector<LeapMotionPlugin::LeapMotionJoint>& joints,
const std::vector<LeapMotionPlugin::LeapMotionJoint>& prevJoints) {
glm::mat4 controllerToAvatar = glm::inverse(inputCalibrationData.avatarMat) * inputCalibrationData.sensorToWorldMat;
glm::quat controllerToAvatarRotation = glmExtractRotation(controllerToAvatar);
glm::vec3 hmdSensorPosition; // HMD
glm::quat hmdSensorOrientation; // HMD
glm::vec3 leapMotionOffset; // Desktop
if (_isLeapOnHMD) {
hmdSensorPosition = extractTranslation(inputCalibrationData.hmdSensorMat);
hmdSensorOrientation = extractRotation(inputCalibrationData.hmdSensorMat);
} else {
// Desktop "zero" position is some distance above the Leap Motion sensor and half the avatar's shoulder-to-hand length
// in front of avatar.
float halfShouldToHandLength = fabsf(extractTranslation(inputCalibrationData.defaultLeftHand).x
- extractTranslation(inputCalibrationData.defaultLeftArm).x) / 2.0f;
leapMotionOffset = glm::vec3(0.0f, _desktopHeightOffset, halfShouldToHandLength);
}
for (size_t i = 0; i < joints.size(); i++) {
int poseIndex = LeapMotionJointIndexToPoseIndex((LeapMotionJointIndex)i);
if (joints[i].position == Vectors::ZERO) {
_poseStateMap[poseIndex] = controller::Pose();
continue;
}
glm::vec3 pos;
glm::quat rot;
if (_isLeapOnHMD) {
auto jointPosition = joints[i].position;
const glm::vec3 HMD_EYE_TO_LEAP_OFFSET = glm::vec3(0.0f, 0.0f, -0.09f); // Eyes to surface of Leap Motion.
jointPosition = glm::vec3(-jointPosition.x, -jointPosition.z, -jointPosition.y) + HMD_EYE_TO_LEAP_OFFSET;
jointPosition = hmdSensorPosition + hmdSensorOrientation * jointPosition;
pos = transformPoint(controllerToAvatar, jointPosition);
glm::quat jointOrientation = joints[i].orientation;
jointOrientation = glm::quat(jointOrientation.w, -jointOrientation.x, -jointOrientation.z, -jointOrientation.y);
rot = controllerToAvatarRotation * hmdSensorOrientation * jointOrientation;
} else {
pos = controllerToAvatarRotation * (joints[i].position - leapMotionOffset);
const glm::quat ZERO_HAND_ORIENTATION = glm::quat(glm::vec3(PI_OVER_TWO, PI, 0.0f));
rot = controllerToAvatarRotation * joints[i].orientation * ZERO_HAND_ORIENTATION;
}
glm::vec3 linearVelocity, angularVelocity;
if (i < prevJoints.size()) {
linearVelocity = (pos - (prevJoints[i].position * METERS_PER_CENTIMETER)) / deltaTime; // m/s
// quat log imaginary part points along the axis of rotation, with length of one half the angle of rotation.
glm::quat d = glm::log(rot * glm::inverse(prevJoints[i].orientation));
angularVelocity = glm::vec3(d.x, d.y, d.z) / (0.5f * deltaTime); // radians/s
}
_poseStateMap[poseIndex] = controller::Pose(pos, rot, linearVelocity, angularVelocity);
}
}
Pose Pose::transform(const glm::mat4& mat) const {
auto rot = glmExtractRotation(mat);
Pose pose(transformPoint(mat, translation),
rot * rotation,
transformVectorFast(mat, velocity),
rot * angularVelocity);
pose.valid = valid;
return pose;
}
// This will attempt to determine the proper body facing of a characters body
// assumes headRot is z-forward and y-up.
// and returns a bodyRot that is also z-forward and y-up
glm::quat computeBodyFacingFromHead(const glm::quat& headRot, const glm::vec3& up) {
glm::vec3 bodyUp = glm::normalize(up);
// initially take the body facing from the head.
glm::vec3 headUp = headRot * Vectors::UNIT_Y;
glm::vec3 headForward = headRot * Vectors::UNIT_Z;
glm::vec3 headLeft = headRot * Vectors::UNIT_X;
const float NOD_THRESHOLD = cosf(glm::radians(45.0f));
const float TILT_THRESHOLD = cosf(glm::radians(30.0f));
glm::vec3 bodyForward = headForward;
float nodDot = glm::dot(headForward, bodyUp);
float tiltDot = glm::dot(headLeft, bodyUp);
if (fabsf(tiltDot) < TILT_THRESHOLD) { // if we are not tilting too much
if (nodDot < -NOD_THRESHOLD) { // head is looking downward
// the body should face in the same direction as the top the head.
bodyForward = headUp;
} else if (nodDot > NOD_THRESHOLD) { // head is looking upward
// the body should face away from the top of the head.
bodyForward = -headUp;
}
}
// cancel out upward component
bodyForward = glm::normalize(bodyForward - nodDot * bodyUp);
glm::vec3 u, v, w;
generateBasisVectors(bodyForward, bodyUp, u, v, w);
// create matrix from orthogonal basis vectors
glm::mat4 bodyMat(glm::vec4(w, 0.0f), glm::vec4(v, 0.0f), glm::vec4(u, 0.0f), glm::vec4(0.0f, 0.0f, 0.0f, 1.0f));
return glmExtractRotation(bodyMat);
}
void KinectPlugin::InputDevice::update(float deltaTime, const controller::InputCalibrationData& inputCalibrationData,
const std::vector<KinectPlugin::KinectJoint>& joints, const std::vector<KinectPlugin::KinectJoint>& prevJoints) {
glm::mat4 controllerToAvatar = glm::inverse(inputCalibrationData.avatarMat) * inputCalibrationData.sensorToWorldMat;
glm::quat controllerToAvatarRotation = glmExtractRotation(controllerToAvatar);
vec3 kinectHipPos;
if (joints.size() > JointType_SpineBase) {
kinectHipPos = joints[JointType_SpineBase].position;
}
for (size_t i = 0; i < joints.size(); i++) {
int poseIndex = KinectJointIndexToPoseIndex((KinectJointIndex)i);
glm::vec3 linearVel, angularVel;
// Adjust the position to be hip (avatar) relative, and rotated to match the avatar rotation
const glm::vec3& pos = controllerToAvatarRotation * (joints[i].position - kinectHipPos);
if (Vectors::ZERO == pos) {
_poseStateMap[poseIndex] = controller::Pose();
continue;
}
// FIXME - determine the correct orientation transform
glm::quat rot = joints[i].orientation;
if (i < prevJoints.size()) {
linearVel = (pos - (prevJoints[i].position * METERS_PER_CENTIMETER)) / deltaTime; // m/s
// quat log imaginary part points along the axis of rotation, with length of one half the angle of rotation.
glm::quat d = glm::log(rot * glm::inverse(prevJoints[i].orientation));
angularVel = glm::vec3(d.x, d.y, d.z) / (0.5f * deltaTime); // radians/s
}
_poseStateMap[poseIndex] = controller::Pose(pos, rot, linearVel, angularVel);
}
}
bool RenderableModelEntityItem::getAnimationFrame() {
bool newFrame = false;
if (!_model || !_model->isActive() || !_model->isLoaded() || _needsInitialSimulation) {
return false;
}
if (!hasAnimation() || !_jointMappingCompleted) {
return false;
}
AnimationPointer myAnimation = getAnimation(_animationProperties.getURL()); // FIXME: this could be optimized
if (myAnimation && myAnimation->isLoaded()) {
const QVector<FBXAnimationFrame>& frames = myAnimation->getFramesReference(); // NOTE: getFrames() is too heavy
auto& fbxJoints = myAnimation->getGeometry().joints;
int frameCount = frames.size();
if (frameCount > 0) {
int animationCurrentFrame = (int)(glm::floor(getAnimationCurrentFrame())) % frameCount;
if (animationCurrentFrame < 0 || animationCurrentFrame > frameCount) {
animationCurrentFrame = 0;
}
if (animationCurrentFrame != _lastKnownCurrentFrame) {
_lastKnownCurrentFrame = animationCurrentFrame;
newFrame = true;
resizeJointArrays();
if (_jointMapping.size() != _model->getJointStateCount()) {
qDebug() << "RenderableModelEntityItem::getAnimationFrame -- joint count mismatch"
<< _jointMapping.size() << _model->getJointStateCount();
assert(false);
return false;
}
const QVector<glm::quat>& rotations = frames[animationCurrentFrame].rotations;
const QVector<glm::vec3>& translations = frames[animationCurrentFrame].translations;
for (int j = 0; j < _jointMapping.size(); j++) {
int index = _jointMapping[j];
if (index >= 0) {
glm::mat4 translationMat;
if (index < translations.size()) {
translationMat = glm::translate(translations[index]);
}
glm::mat4 rotationMat(glm::mat4::_null);
if (index < rotations.size()) {
rotationMat = glm::mat4_cast(fbxJoints[index].preRotation * rotations[index] * fbxJoints[index].postRotation);
} else {
rotationMat = glm::mat4_cast(fbxJoints[index].preRotation * fbxJoints[index].postRotation);
}
glm::mat4 finalMat = (translationMat * fbxJoints[index].preTransform *
rotationMat * fbxJoints[index].postTransform);
_absoluteJointTranslationsInObjectFrame[j] = extractTranslation(finalMat);
_absoluteJointTranslationsInObjectFrameSet[j] = true;
_absoluteJointTranslationsInObjectFrameDirty[j] = true;
_absoluteJointRotationsInObjectFrame[j] = glmExtractRotation(finalMat);
_absoluteJointRotationsInObjectFrameSet[j] = true;
_absoluteJointRotationsInObjectFrameDirty[j] = true;
}
}
}
}
}
return newFrame;
}
