Mirrors/overte-lubosz
3
0
Fork 0
mirror of https://github.com/lubosz/overte.git synced 2025-04-14 03:06:20 +02:00
Code Issues Projects Releases Packages Wiki Activity Actions

Merge pull request #10273 from hyperlogic/feature/hips-ik-target

Browse source 
Avatar IK system now supports manipulating the hips.
This commit is contained in:
Seth Alves 2017-04-28 09:17:33 -07:00 committed by GitHub
commit 2a0fc5b34c
19 changed files with 786 additions and 200 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

31
interface/resources/avatar/avatar-animation.json
View file

@ -50,6 +50,12 @@
"type": "inverseKinematics",
"data": {
"targets": [
{
"jointName": "Hips",
"positionVar": "hipsPosition",
"rotationVar": "hipsRotation",
"typeVar": "hipsType"
},
{
"jointName": "RightHand",
"positionVar": "rightHandPosition",
@ -75,10 +81,10 @@
"typeVar": "leftFootType"
},
{
"jointName": "Neck",
"positionVar": "neckPosition",
"rotationVar": "neckRotation",
"typeVar": "neckType"
"jointName": "Spine2",
"positionVar": "spine2Position",
"rotationVar": "spine2Rotation",
"typeVar": "spine2Type"
},
{
"jointName": "Head",
@ -91,20 +97,27 @@
"children": []
},
{
"id": "manipulatorOverlay",
"id": "hipsManipulatorOverlay",
"type": "overlay",
"data": {
"alpha": 1.0,
"boneSet": "spineOnly"
"alpha": 0.0,
"boneSet": "hipsOnly"
},
"children": [
{
"id": "spineLean",
"id": "hipsManipulator",
"type": "manipulator",
"data": {
"alpha": 0.0,
"alphaVar": "hipsManipulatorAlpha",
"joints": [
{ "type": "absoluteRotation", "jointName": "Spine", "var": "lean" }
{
"jointName": "Hips",
"rotationType": "absolute",
"translationType": "absolute",
"rotationVar": "hipsManipulatorRotation",
"translationVar": "hipsManipulatorPosition"
}
]
},
"children": []

8
interface/src/avatar/MyAvatar.h
View file

@ -465,6 +465,10 @@ public:
void removeHoldAction(AvatarActionHold* holdAction); // thread-safe
void updateHoldActions(const AnimPose& prePhysicsPose, const AnimPose& postUpdatePose);
// derive avatar body position and orientation from the current HMD Sensor location.
// results are in HMD frame
glm::mat4 deriveBodyFromHMDSensor() const;
public slots:
void increaseSize();
void decreaseSize();
@ -553,9 +557,7 @@ private:
void setVisibleInSceneIfReady(Model* model, const render::ScenePointer& scene, bool visiblity);
// derive avatar body position and orientation from the current HMD Sensor location.
// results are in HMD frame
glm::mat4 deriveBodyFromHMDSensor() const;
private:
virtual void updatePalms() override {}
void lateUpdatePalms();

6
interface/src/avatar/SkeletonModel.cpp
View file

@ -119,7 +119,13 @@ void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) {
headParams.rigHeadPosition = extractTranslation(rigHMDMat);
headParams.rigHeadOrientation = extractRotation(rigHMDMat);
headParams.worldHeadOrientation = extractRotation(worldHMDMat);
// TODO: if hips target sensor is valid.
// Copy it into headParams.hipsMatrix, and set headParams.hipsEnabled to true.
headParams.hipsEnabled = false;
} else {
headParams.hipsEnabled = false;
headParams.isInHMD = false;
// We don't have a valid localHeadPosition.

296
libraries/animation/src/AnimInverseKinematics.cpp
View file

@ -86,7 +86,9 @@ void AnimInverseKinematics::setTargetVars(
void AnimInverseKinematics::computeTargets(const AnimVariantMap& animVars, std::vector<IKTarget>& targets, const AnimPoseVec& underPoses) {
// build a list of valid targets from _targetVarVec and animVars
_maxTargetIndex = -1;
_hipsTargetIndex = -1;
bool removeUnfoundJoints = false;
for (auto& targetVar : _targetVarVec) {
if (targetVar.jointIndex == -1) {
// this targetVar hasn't been validated yet...
@ -105,15 +107,18 @@ void AnimInverseKinematics::computeTargets(const AnimVariantMap& animVars, std::
AnimPose defaultPose = _skeleton->getAbsolutePose(targetVar.jointIndex, underPoses);
glm::quat rotation = animVars.lookupRigToGeometry(targetVar.rotationVar, defaultPose.rot());
glm::vec3 translation = animVars.lookupRigToGeometry(targetVar.positionVar, defaultPose.trans());
if (target.getType() == IKTarget::Type::HipsRelativeRotationAndPosition) {
translation += _hipsOffset;
}
target.setPose(rotation, translation);
target.setIndex(targetVar.jointIndex);
targets.push_back(target);
if (targetVar.jointIndex > _maxTargetIndex) {
_maxTargetIndex = targetVar.jointIndex;
}
// record the index of the hips ik target.
if (target.getIndex() == _hipsIndex) {
_hipsTargetIndex = (int)targets.size() - 1;
}
}
}
}
@ -242,18 +247,21 @@ int AnimInverseKinematics::solveTargetWithCCD(const IKTarget& target, AnimPoseVe
// the tip's parent-relative as we proceed up the chain
glm::quat tipParentOrientation = absolutePoses[pivotIndex].rot();
// NOTE: if this code is removed, the head will remain rigid, causing the spine/hips to thrust forward backward
// as the head is nodded.
if (targetType == IKTarget::Type::HmdHead) {
// rotate tip directly to target orientation
tipOrientation = target.getRotation();
glm::quat tipRelativeRotation = glm::normalize(tipOrientation * glm::inverse(tipParentOrientation));
glm::quat tipRelativeRotation = glm::inverse(tipParentOrientation) * tipOrientation;
// enforce tip's constraint
// then enforce tip's constraint
RotationConstraint* constraint = getConstraint(tipIndex);
if (constraint) {
bool constrained = constraint->apply(tipRelativeRotation);
if (constrained) {
tipOrientation = glm::normalize(tipRelativeRotation * tipParentOrientation);
tipRelativeRotation = glm::normalize(tipOrientation * glm::inverse(tipParentOrientation));
tipOrientation = tipParentOrientation * tipRelativeRotation;
tipRelativeRotation = tipRelativeRotation;
}
}
// store the relative rotation change in the accumulator
@ -277,7 +285,9 @@ int AnimInverseKinematics::solveTargetWithCCD(const IKTarget& target, AnimPoseVe
const float MIN_AXIS_LENGTH = 1.0e-4f;
RotationConstraint* constraint = getConstraint(pivotIndex);
if (constraint && constraint->isLowerSpine() && tipIndex != _headIndex) {
// only allow swing on lowerSpine if there is a hips IK target.
if (_hipsTargetIndex < 0 && constraint && constraint->isLowerSpine() && tipIndex != _headIndex) {
// for these types of targets we only allow twist at the lower-spine
// (this prevents the hand targets from bending the spine too much and thereby driving the hips too far)
glm::vec3 twistAxis = absolutePoses[pivotIndex].trans() - absolutePoses[pivotsParentIndex].trans();
@ -420,13 +430,13 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars
_relativePoses[i].trans() = underPoses[i].trans();
}
if (!_relativePoses.empty()) {
if (!underPoses.empty()) {
// Sometimes the underpose itself can violate the constraints. Rather than
// clamp the animation we dynamically expand each constraint to accomodate it.
std::map<int, RotationConstraint*>::iterator constraintItr = _constraints.begin();
while (constraintItr != _constraints.end()) {
int index = constraintItr->first;
constraintItr->second->dynamicallyAdjustLimits(_relativePoses[index].rot());
constraintItr->second->dynamicallyAdjustLimits(underPoses[index].rot());
++constraintItr;
}
}
@ -441,64 +451,76 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars
computeTargets(animVars, targets, underPoses);
}
// debug render ik targets
if (context.getEnableDebugDrawIKTargets()) {
const vec4 WHITE(1.0f);
glm::mat4 rigToAvatarMat = createMatFromQuatAndPos(Quaternions::Y_180, glm::vec3());
for (auto& target : targets) {
glm::mat4 geomTargetMat = createMatFromQuatAndPos(target.getRotation(), target.getTranslation());
glm::mat4 avatarTargetMat = rigToAvatarMat * context.getGeometryToRigMatrix() * geomTargetMat;
QString name = QString("ikTarget%1").arg(target.getIndex());
DebugDraw::getInstance().addMyAvatarMarker(name, glmExtractRotation(avatarTargetMat), extractTranslation(avatarTargetMat), WHITE);
}
} else if (context.getEnableDebugDrawIKTargets() != _previousEnableDebugIKTargets) {
// remove markers if they were added last frame.
for (auto& target : targets) {
QString name = QString("ikTarget%1").arg(target.getIndex());
DebugDraw::getInstance().removeMyAvatarMarker(name);
}
}
_previousEnableDebugIKTargets = context.getEnableDebugDrawIKTargets();
if (targets.empty()) {
// no IK targets but still need to enforce constraints
std::map<int, RotationConstraint*>::iterator constraintItr = _constraints.begin();
while (constraintItr != _constraints.end()) {
int index = constraintItr->first;
glm::quat rotation = _relativePoses[index].rot();
constraintItr->second->apply(rotation);
_relativePoses[index].rot() = rotation;
++constraintItr;
}
_relativePoses = underPoses;
} else {
{
PROFILE_RANGE_EX(simulation_animation, "ik/shiftHips", 0xffff00ff, 0);
// shift hips according to the _hipsOffset from the previous frame
float offsetLength = glm::length(_hipsOffset);
const float MIN_HIPS_OFFSET_LENGTH = 0.03f;
if (offsetLength > MIN_HIPS_OFFSET_LENGTH && _hipsIndex >= 0) {
// but only if offset is long enough
float scaleFactor = ((offsetLength - MIN_HIPS_OFFSET_LENGTH) / offsetLength);
if (_hipsParentIndex == -1) {
// the hips are the root so _hipsOffset is in the correct frame
_relativePoses[_hipsIndex].trans() = underPoses[_hipsIndex].trans() + scaleFactor * _hipsOffset;
if (_hipsTargetIndex >= 0 && _hipsTargetIndex < (int)targets.size()) {
// slam the hips to match the _hipsTarget
AnimPose absPose = targets[_hipsTargetIndex].getPose();
int parentIndex = _skeleton->getParentIndex(targets[_hipsTargetIndex].getIndex());
if (parentIndex != -1) {
_relativePoses[_hipsIndex] = _skeleton->getAbsolutePose(parentIndex, _relativePoses).inverse() * absPose;
} else {
// the hips are NOT the root so we need to transform _hipsOffset into hips local-frame
glm::quat hipsFrameRotation = _relativePoses[_hipsParentIndex].rot();
int index = _skeleton->getParentIndex(_hipsParentIndex);
while (index != -1) {
hipsFrameRotation *= _relativePoses[index].rot();
index = _skeleton->getParentIndex(index);
_relativePoses[_hipsIndex] = absPose;
}
} else {
// if there is no hips target, shift hips according to the _hipsOffset from the previous frame
float offsetLength = glm::length(_hipsOffset);
const float MIN_HIPS_OFFSET_LENGTH = 0.03f;
if (offsetLength > MIN_HIPS_OFFSET_LENGTH && _hipsIndex >= 0) {
float scaleFactor = ((offsetLength - MIN_HIPS_OFFSET_LENGTH) / offsetLength);
glm::vec3 hipsOffset = scaleFactor * _hipsOffset;
if (_hipsParentIndex == -1) {
_relativePoses[_hipsIndex].trans() = underPoses[_hipsIndex].trans() + hipsOffset;
} else {
auto absHipsPose = _skeleton->getAbsolutePose(_hipsIndex, underPoses);
absHipsPose.trans() += hipsOffset;
_relativePoses[_hipsIndex] = _skeleton->getAbsolutePose(_hipsParentIndex, _relativePoses).inverse() * absHipsPose;
}
_relativePoses[_hipsIndex].trans() = underPoses[_hipsIndex].trans()
+ glm::inverse(glm::normalize(hipsFrameRotation)) * (scaleFactor * _hipsOffset);
}
}
// update all HipsRelative targets to account for the hips shift/ik target.
auto shiftedHipsAbsPose = _skeleton->getAbsolutePose(_hipsIndex, _relativePoses);
auto underHipsAbsPose = _skeleton->getAbsolutePose(_hipsIndex, underPoses);
auto absHipsOffset = shiftedHipsAbsPose.trans() - underHipsAbsPose.trans();
for (auto& target: targets) {
if (target.getType() == IKTarget::Type::HipsRelativeRotationAndPosition) {
auto pose = target.getPose();
pose.trans() = pose.trans() + absHipsOffset;
target.setPose(pose.rot(), pose.trans());
}
}
}
{
PROFILE_RANGE_EX(simulation_animation, "ik/debugDraw", 0xffff00ff, 0);
// debug render ik targets
if (context.getEnableDebugDrawIKTargets()) {
const vec4 WHITE(1.0f);
glm::mat4 rigToAvatarMat = createMatFromQuatAndPos(Quaternions::Y_180, glm::vec3());
for (auto& target : targets) {
glm::mat4 geomTargetMat = createMatFromQuatAndPos(target.getRotation(), target.getTranslation());
glm::mat4 avatarTargetMat = rigToAvatarMat * context.getGeometryToRigMatrix() * geomTargetMat;
QString name = QString("ikTarget%1").arg(target.getIndex());
DebugDraw::getInstance().addMyAvatarMarker(name, glmExtractRotation(avatarTargetMat), extractTranslation(avatarTargetMat), WHITE);
}
} else if (context.getEnableDebugDrawIKTargets() != _previousEnableDebugIKTargets) {
// remove markers if they were added last frame.
for (auto& target : targets) {
QString name = QString("ikTarget%1").arg(target.getIndex());
DebugDraw::getInstance().removeMyAvatarMarker(name);
}
}
_previousEnableDebugIKTargets = context.getEnableDebugDrawIKTargets();
}
{
@ -506,64 +528,70 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars
solveWithCyclicCoordinateDescent(targets);
}
{
if (_hipsTargetIndex < 0) {
PROFILE_RANGE_EX(simulation_animation, "ik/measureHipsOffset", 0xffff00ff, 0);
// measure new _hipsOffset for next frame
// by looking for discrepancies between where a targeted endEffector is
// and where it wants to be (after IK solutions are done)
glm::vec3 newHipsOffset = Vectors::ZERO;
for (auto& target: targets) {
int targetIndex = target.getIndex();
if (targetIndex == _headIndex && _headIndex != -1) {
// special handling for headTarget
if (target.getType() == IKTarget::Type::RotationOnly) {
// we want to shift the hips to bring the underPose closer
// to where the head happens to be (overpose)
glm::vec3 under = _skeleton->getAbsolutePose(_headIndex, underPoses).trans();
glm::vec3 actual = _skeleton->getAbsolutePose(_headIndex, _relativePoses).trans();
const float HEAD_OFFSET_SLAVE_FACTOR = 0.65f;
newHipsOffset += HEAD_OFFSET_SLAVE_FACTOR * (actual - under);
} else if (target.getType() == IKTarget::Type::HmdHead) {
// we want to shift the hips to bring the head to its designated position
glm::vec3 actual = _skeleton->getAbsolutePose(_headIndex, _relativePoses).trans();
_hipsOffset += target.getTranslation() - actual;
// and ignore all other targets
newHipsOffset = _hipsOffset;
break;
} else if (target.getType() == IKTarget::Type::RotationAndPosition) {
glm::vec3 actualPosition = _skeleton->getAbsolutePose(targetIndex, _relativePoses).trans();
glm::vec3 targetPosition = target.getTranslation();
newHipsOffset += targetPosition - actualPosition;
// Add downward pressure on the hips
newHipsOffset *= 0.95f;
newHipsOffset -= 1.0f;
}
} else if (target.getType() == IKTarget::Type::RotationAndPosition) {
glm::vec3 actualPosition = _skeleton->getAbsolutePose(targetIndex, _relativePoses).trans();
glm::vec3 targetPosition = target.getTranslation();
newHipsOffset += targetPosition - actualPosition;
}
}
// smooth transitions by relaxing _hipsOffset toward the new value
const float HIPS_OFFSET_SLAVE_TIMESCALE = 0.10f;
float tau = dt < HIPS_OFFSET_SLAVE_TIMESCALE ? dt / HIPS_OFFSET_SLAVE_TIMESCALE : 1.0f;
_hipsOffset += (newHipsOffset - _hipsOffset) * tau;
// clamp the hips offset
float hipsOffsetLength = glm::length(_hipsOffset);
if (hipsOffsetLength > _maxHipsOffsetLength) {
_hipsOffset *= _maxHipsOffsetLength / hipsOffsetLength;
}
computeHipsOffset(targets, underPoses, dt);
} else {
_hipsOffset = Vectors::ZERO;
}
}
}
return _relativePoses;
}
void AnimInverseKinematics::computeHipsOffset(const std::vector<IKTarget>& targets, const AnimPoseVec& underPoses, float dt) {
// measure new _hipsOffset for next frame
// by looking for discrepancies between where a targeted endEffector is
// and where it wants to be (after IK solutions are done)
glm::vec3 newHipsOffset = Vectors::ZERO;
for (auto& target: targets) {
int targetIndex = target.getIndex();
if (targetIndex == _headIndex && _headIndex != -1) {
// special handling for headTarget
if (target.getType() == IKTarget::Type::RotationOnly) {
// we want to shift the hips to bring the underPose closer
// to where the head happens to be (overpose)
glm::vec3 under = _skeleton->getAbsolutePose(_headIndex, underPoses).trans();
glm::vec3 actual = _skeleton->getAbsolutePose(_headIndex, _relativePoses).trans();
const float HEAD_OFFSET_SLAVE_FACTOR = 0.65f;
newHipsOffset += HEAD_OFFSET_SLAVE_FACTOR * (actual - under);
} else if (target.getType() == IKTarget::Type::HmdHead) {
// we want to shift the hips to bring the head to its designated position
glm::vec3 actual = _skeleton->getAbsolutePose(_headIndex, _relativePoses).trans();
_hipsOffset += target.getTranslation() - actual;
// and ignore all other targets
newHipsOffset = _hipsOffset;
break;
} else if (target.getType() == IKTarget::Type::RotationAndPosition) {
glm::vec3 actualPosition = _skeleton->getAbsolutePose(targetIndex, _relativePoses).trans();
glm::vec3 targetPosition = target.getTranslation();
newHipsOffset += targetPosition - actualPosition;
// Add downward pressure on the hips
const float PRESSURE_SCALE_FACTOR = 0.95f;
const float PRESSURE_TRANSLATION_OFFSET = 1.0f;
newHipsOffset *= PRESSURE_SCALE_FACTOR;
newHipsOffset -= PRESSURE_TRANSLATION_OFFSET;
}
} else if (target.getType() == IKTarget::Type::RotationAndPosition) {
glm::vec3 actualPosition = _skeleton->getAbsolutePose(targetIndex, _relativePoses).trans();
glm::vec3 targetPosition = target.getTranslation();
newHipsOffset += targetPosition - actualPosition;
}
}
// smooth transitions by relaxing _hipsOffset toward the new value
const float HIPS_OFFSET_SLAVE_TIMESCALE = 0.10f;
float tau = dt < HIPS_OFFSET_SLAVE_TIMESCALE ? dt / HIPS_OFFSET_SLAVE_TIMESCALE : 1.0f;
_hipsOffset += (newHipsOffset - _hipsOffset) * tau;
// clamp the hips offset
float hipsOffsetLength = glm::length(_hipsOffset);
if (hipsOffsetLength > _maxHipsOffsetLength) {
_hipsOffset *= _maxHipsOffsetLength / hipsOffsetLength;
}
}
void AnimInverseKinematics::setMaxHipsOffsetLength(float maxLength) {
// manually adjust scale here
const float METERS_TO_CENTIMETERS = 100.0f;
@ -594,6 +622,22 @@ void AnimInverseKinematics::clearConstraints() {
_constraints.clear();
}
// set up swing limits around a swingTwistConstraint in an ellipse, where lateralSwingTheta is the swing limit for lateral swings (side to side)
// anteriorSwingTheta is swing limit for forward and backward swings. (where x-axis of reference rotation is sideways and -z-axis is forward)
static void setEllipticalSwingLimits(SwingTwistConstraint* stConstraint, float lateralSwingTheta, float anteriorSwingTheta) {
assert(stConstraint);
const int NUM_SUBDIVISIONS = 8;
std::vector<float> minDots;
minDots.reserve(NUM_SUBDIVISIONS);
float dTheta = TWO_PI / NUM_SUBDIVISIONS;
float theta = 0.0f;
for (int i = 0; i < NUM_SUBDIVISIONS; i++) {
minDots.push_back(cosf(glm::length(glm::vec2(anteriorSwingTheta * cosf(theta), lateralSwingTheta * sinf(theta)))));
theta += dTheta;
}
stConstraint->setSwingLimits(minDots);
}
void AnimInverseKinematics::initConstraints() {
if (!_skeleton) {
return;
@ -783,41 +827,31 @@ void AnimInverseKinematics::initConstraints() {
} else if (baseName.startsWith("Spine", Qt::CaseSensitive)) {
SwingTwistConstraint* stConstraint = new SwingTwistConstraint();
stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot());
const float MAX_SPINE_TWIST = PI / 12.0f;
const float MAX_SPINE_TWIST = PI / 20.0f;
stConstraint->setTwistLimits(-MAX_SPINE_TWIST, MAX_SPINE_TWIST);
std::vector<float> minDots;
const float MAX_SPINE_SWING = PI / 10.0f;
minDots.push_back(cosf(MAX_SPINE_SWING));
stConstraint->setSwingLimits(minDots);
// limit lateral swings more then forward-backward swings
const float MAX_SPINE_LATERAL_SWING = PI / 30.0f;
const float MAX_SPINE_ANTERIOR_SWING = PI / 20.0f;
setEllipticalSwingLimits(stConstraint, MAX_SPINE_LATERAL_SWING, MAX_SPINE_ANTERIOR_SWING);
if (0 == baseName.compare("Spine1", Qt::CaseSensitive)
|| 0 == baseName.compare("Spine", Qt::CaseSensitive)) {
stConstraint->setLowerSpine(true);
}
constraint = static_cast<RotationConstraint*>(stConstraint);
} else if (baseName.startsWith("Hips2", Qt::CaseSensitive)) {
SwingTwistConstraint* stConstraint = new SwingTwistConstraint();
stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot());
const float MAX_SPINE_TWIST = PI / 8.0f;
stConstraint->setTwistLimits(-MAX_SPINE_TWIST, MAX_SPINE_TWIST);
std::vector<float> minDots;
const float MAX_SPINE_SWING = PI / 14.0f;
minDots.push_back(cosf(MAX_SPINE_SWING));
stConstraint->setSwingLimits(minDots);
constraint = static_cast<RotationConstraint*>(stConstraint);
} else if (0 == baseName.compare("Neck", Qt::CaseSensitive)) {
SwingTwistConstraint* stConstraint = new SwingTwistConstraint();
stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot());
const float MAX_NECK_TWIST = PI / 9.0f;
const float MAX_NECK_TWIST = PI / 10.0f;
stConstraint->setTwistLimits(-MAX_NECK_TWIST, MAX_NECK_TWIST);
std::vector<float> minDots;
const float MAX_NECK_SWING = PI / 8.0f;
minDots.push_back(cosf(MAX_NECK_SWING));
stConstraint->setSwingLimits(minDots);
// limit lateral swings more then forward-backward swings
const float MAX_NECK_LATERAL_SWING = PI / 10.0f;
const float MAX_NECK_ANTERIOR_SWING = PI / 8.0f;
setEllipticalSwingLimits(stConstraint, MAX_NECK_LATERAL_SWING, MAX_NECK_ANTERIOR_SWING);
constraint = static_cast<RotationConstraint*>(stConstraint);
} else if (0 == baseName.compare("Head", Qt::CaseSensitive)) {
@ -872,7 +906,7 @@ void AnimInverseKinematics::initConstraints() {
// we determine the max/min angles by rotating the swing limit lines from parent- to child-frame
// then measure the angles to swing the yAxis into alignment
const float MIN_KNEE_ANGLE = 0.0f;
const float MIN_KNEE_ANGLE = 0.097f; // ~5 deg
const float MAX_KNEE_ANGLE = 7.0f * PI / 8.0f;
glm::quat invReferenceRotation = glm::inverse(referenceRotation);
glm::vec3 minSwingAxis = invReferenceRotation * glm::angleAxis(MIN_KNEE_ANGLE, hingeAxis) * Vectors::UNIT_Y;

2
libraries/animation/src/AnimInverseKinematics.h
View file

@ -55,6 +55,7 @@ protected:
RotationConstraint* getConstraint(int index);
void clearConstraints();
void initConstraints();
void computeHipsOffset(const std::vector<IKTarget>& targets, const AnimPoseVec& underPoses, float dt);
// no copies
AnimInverseKinematics(const AnimInverseKinematics&) = delete;
@ -91,6 +92,7 @@ protected:
int _headIndex { -1 };
int _hipsIndex { -1 };
int _hipsParentIndex { -1 };
int _hipsTargetIndex { -1 };
// _maxTargetIndex is tracked to help optimize the recalculation of absolute poses
// during the the cyclic coordinate descent algorithm

95
libraries/animation/src/AnimManipulator.cpp
View file

@ -12,6 +12,16 @@
#include "AnimUtil.h"
#include "AnimationLogging.h"
AnimManipulator::JointVar::JointVar(const QString& jointNameIn, Type rotationTypeIn, Type translationTypeIn,
const QString& rotationVarIn, const QString& translationVarIn) :
jointName(jointNameIn),
rotationType(rotationTypeIn),
translationType(translationTypeIn),
rotationVar(rotationVarIn),
translationVar(translationVarIn),
jointIndex(-1),
hasPerformedJointLookup(false) {}
AnimManipulator::AnimManipulator(const QString& id, float alpha) :
AnimNode(AnimNode::Type::Manipulator, id),
_alpha(alpha) {
@ -36,7 +46,10 @@ const AnimPoseVec& AnimManipulator::overlay(const AnimVariantMap& animVars, cons
}
for (auto& jointVar : _jointVars) {
if (!jointVar.hasPerformedJointLookup) {
// map from joint name to joint index and cache the result.
jointVar.jointIndex = _skeleton->nameToJointIndex(jointVar.jointName);
if (jointVar.jointIndex < 0) {
qCWarning(animation) << "AnimManipulator could not find jointName" << jointVar.jointName << "in skeleton";
@ -100,34 +113,62 @@ AnimPose AnimManipulator::computeRelativePoseFromJointVar(const AnimVariantMap&
AnimPose defaultAbsPose = _skeleton->getAbsolutePose(jointVar.jointIndex, underPoses);
if (jointVar.type == JointVar::Type::AbsoluteRotation || jointVar.type == JointVar::Type::AbsolutePosition) {
// compute relative translation
glm::vec3 relTrans;
switch (jointVar.translationType) {
case JointVar::Type::Absolute: {
glm::vec3 absTrans = animVars.lookupRigToGeometry(jointVar.translationVar, defaultAbsPose.trans());
if (jointVar.type == JointVar::Type::AbsoluteRotation) {
defaultAbsPose.rot() = animVars.lookupRigToGeometry(jointVar.var, defaultAbsPose.rot());
} else if (jointVar.type == JointVar::Type::AbsolutePosition) {
defaultAbsPose.trans() = animVars.lookupRigToGeometry(jointVar.var, defaultAbsPose.trans());
// convert to from absolute to relative.
AnimPose parentAbsPose;
int parentIndex = _skeleton->getParentIndex(jointVar.jointIndex);
if (parentIndex >= 0) {
parentAbsPose = _skeleton->getAbsolutePose(parentIndex, underPoses);
}
// convert from absolute to relative
relTrans = transformPoint(parentAbsPose.inverse(), absTrans);
break;
}
// because jointVar is absolute, we must use an absolute parent frame to convert into a relative pose.
AnimPose parentAbsPose = AnimPose::identity;
int parentIndex = _skeleton->getParentIndex(jointVar.jointIndex);
if (parentIndex >= 0) {
parentAbsPose = _skeleton->getAbsolutePose(parentIndex, underPoses);
}
// convert from absolute to relative
return parentAbsPose.inverse() * defaultAbsPose;
} else {
// override the default rel pose
AnimPose relPose = defaultRelPose;
if (jointVar.type == JointVar::Type::RelativeRotation) {
relPose.rot() = animVars.lookupRigToGeometry(jointVar.var, defaultRelPose.rot());
} else if (jointVar.type == JointVar::Type::RelativePosition) {
relPose.trans() = animVars.lookupRigToGeometry(jointVar.var, defaultRelPose.trans());
}
return relPose;
case JointVar::Type::Relative:
relTrans = animVars.lookupRigToGeometryVector(jointVar.translationVar, defaultRelPose.trans());
break;
case JointVar::Type::UnderPose:
relTrans = underPoses[jointVar.jointIndex].trans();
break;
case JointVar::Type::Default:
default:
relTrans = defaultRelPose.trans();
break;
}
glm::quat relRot;
switch (jointVar.rotationType) {
case JointVar::Type::Absolute: {
glm::quat absRot = animVars.lookupRigToGeometry(jointVar.translationVar, defaultAbsPose.rot());
// convert to from absolute to relative.
AnimPose parentAbsPose;
int parentIndex = _skeleton->getParentIndex(jointVar.jointIndex);
if (parentIndex >= 0) {
parentAbsPose = _skeleton->getAbsolutePose(parentIndex, underPoses);
}
// convert from absolute to relative
relRot = glm::inverse(parentAbsPose.rot()) * absRot;
break;
}
case JointVar::Type::Relative:
relRot = animVars.lookupRigToGeometry(jointVar.translationVar, defaultRelPose.rot());
break;
case JointVar::Type::UnderPose:
relRot = underPoses[jointVar.jointIndex].rot();
break;
case JointVar::Type::Default:
default:
relRot = defaultRelPose.rot();
break;
}
return AnimPose(glm::vec3(1), relRot, relTrans);
}

17
libraries/animation/src/AnimManipulator.h
View file

@ -31,17 +31,20 @@ public:
struct JointVar {
enum class Type {
AbsoluteRotation = 0,
AbsolutePosition,
RelativeRotation,
RelativePosition,
Absolute,
Relative,
UnderPose,
Default,
NumTypes
};
JointVar(const QString& varIn, const QString& jointNameIn, Type typeIn) : var(varIn), jointName(jointNameIn), type(typeIn), jointIndex(-1), hasPerformedJointLookup(false) {}
QString var = "";
JointVar(const QString& jointNameIn, Type rotationType, Type translationType, const QString& rotationVarIn, const QString& translationVarIn);
QString jointName = "";
Type type = Type::AbsoluteRotation;
Type rotationType = Type::Absolute;
Type translationType = Type::Absolute;
QString rotationVar = "";
QString translationVar = "";
int jointIndex = -1;
bool hasPerformedJointLookup = false;
bool isRelative = false;

33
libraries/animation/src/AnimNodeLoader.cpp
View file

@ -79,10 +79,10 @@ static AnimStateMachine::InterpType stringToInterpType(const QString& str) {
static const char* animManipulatorJointVarTypeToString(AnimManipulator::JointVar::Type type) {
switch (type) {
case AnimManipulator::JointVar::Type::AbsoluteRotation: return "absoluteRotation";
case AnimManipulator::JointVar::Type::AbsolutePosition: return "absolutePosition";
case AnimManipulator::JointVar::Type::RelativeRotation: return "relativeRotation";
case AnimManipulator::JointVar::Type::RelativePosition: return "relativePosition";
case AnimManipulator::JointVar::Type::Absolute: return "absolute";
case AnimManipulator::JointVar::Type::Relative: return "relative";
case AnimManipulator::JointVar::Type::UnderPose: return "underPose";
case AnimManipulator::JointVar::Type::Default: return "default";
case AnimManipulator::JointVar::Type::NumTypes: return nullptr;
};
return nullptr;
@ -339,7 +339,8 @@ static const char* boneSetStrings[AnimOverlay::NumBoneSets] = {
"spineOnly",
"empty",
"leftHand",
"rightHand"
"rightHand",
"hipsOnly"
};
static AnimOverlay::BoneSet stringToBoneSetEnum(const QString& str) {
@ -406,17 +407,25 @@ static AnimNode::Pointer loadManipulatorNode(const QJsonObject& jsonObj, const Q
}
auto jointObj = jointValue.toObject();
READ_STRING(type, jointObj, id, jsonUrl, nullptr);
READ_STRING(jointName, jointObj, id, jsonUrl, nullptr);
READ_STRING(var, jointObj, id, jsonUrl, nullptr);
READ_STRING(rotationType, jointObj, id, jsonUrl, nullptr);
READ_STRING(translationType, jointObj, id, jsonUrl, nullptr);
READ_STRING(rotationVar, jointObj, id, jsonUrl, nullptr);
READ_STRING(translationVar, jointObj, id, jsonUrl, nullptr);
AnimManipulator::JointVar::Type jointVarType = stringToAnimManipulatorJointVarType(type);
if (jointVarType == AnimManipulator::JointVar::Type::NumTypes) {
qCCritical(animation) << "AnimNodeLoader, bad type in \"joints\", id =" << id << ", url =" << jsonUrl.toDisplayString();
return nullptr;
AnimManipulator::JointVar::Type jointVarRotationType = stringToAnimManipulatorJointVarType(rotationType);
if (jointVarRotationType == AnimManipulator::JointVar::Type::NumTypes) {
qCWarning(animation) << "AnimNodeLoader, bad rotationType in \"joints\", id =" << id << ", url =" << jsonUrl.toDisplayString();
jointVarRotationType = AnimManipulator::JointVar::Type::Default;
}
AnimManipulator::JointVar jointVar(var, jointName, jointVarType);
AnimManipulator::JointVar::Type jointVarTranslationType = stringToAnimManipulatorJointVarType(translationType);
if (jointVarTranslationType == AnimManipulator::JointVar::Type::NumTypes) {
qCWarning(animation) << "AnimNodeLoader, bad translationType in \"joints\", id =" << id << ", url =" << jsonUrl.toDisplayString();
jointVarTranslationType = AnimManipulator::JointVar::Type::Default;
}
AnimManipulator::JointVar jointVar(jointName, jointVarRotationType, jointVarTranslationType, rotationVar, translationVar);
node->addJointVar(jointVar);
};

8
libraries/animation/src/AnimOverlay.cpp
View file

@ -34,6 +34,7 @@ void AnimOverlay::buildBoneSet(BoneSet boneSet) {
case SpineOnlyBoneSet: buildSpineOnlyBoneSet(); break;
case LeftHandBoneSet: buildLeftHandBoneSet(); break;
case RightHandBoneSet: buildRightHandBoneSet(); break;
case HipsOnlyBoneSet: buildHipsOnlyBoneSet(); break;
default:
case EmptyBoneSet: buildEmptyBoneSet(); break;
}
@ -188,6 +189,13 @@ void AnimOverlay::buildRightHandBoneSet() {
});
}
void AnimOverlay::buildHipsOnlyBoneSet() {
assert(_skeleton);
buildEmptyBoneSet();
int hipsJoint = _skeleton->nameToJointIndex("Hips");
_boneSetVec[hipsJoint] = 1.0f;
}
// for AnimDebugDraw rendering
const AnimPoseVec& AnimOverlay::getPosesInternal() const {
return _poses;

2
libraries/animation/src/AnimOverlay.h
View file

@ -37,6 +37,7 @@ public:
EmptyBoneSet,
LeftHandBoneSet,
RightHandBoneSet,
HipsOnlyBoneSet,
NumBoneSets
};
@ -75,6 +76,7 @@ public:
void buildEmptyBoneSet();
void buildLeftHandBoneSet();
void buildRightHandBoneSet();
void buildHipsOnlyBoneSet();
// no copies
AnimOverlay(const AnimOverlay&) = delete;

9
libraries/animation/src/AnimVariant.h
View file

@ -165,6 +165,15 @@ public:
}
}
glm::vec3 lookupRigToGeometryVector(const QString& key, const glm::vec3& defaultValue) const {
if (key.isEmpty()) {
return defaultValue;
} else {
auto iter = _map.find(key);
return iter != _map.end() ? transformVectorFast(_rigToGeometryMat, iter->second.getVec3()) : defaultValue;
}
}
const glm::quat& lookupRaw(const QString& key, const glm::quat& defaultValue) const {
if (key.isEmpty()) {
return defaultValue;

3
libraries/animation/src/IKTarget.h
View file

@ -21,13 +21,14 @@ public:
RotationOnly,
HmdHead,
HipsRelativeRotationAndPosition,
Unknown,
Unknown
};
IKTarget() {}
const glm::vec3& getTranslation() const { return _pose.trans(); }
const glm::quat& getRotation() const { return _pose.rot(); }
const AnimPose& getPose() const { return _pose; }
int getIndex() const { return _index; }
Type getType() const { return _type; }

36
libraries/animation/src/Rig.cpp
View file

@ -1024,6 +1024,17 @@ void Rig::updateFromHeadParameters(const HeadParameters& params, float dt) {
_animVars.set("isTalking", params.isTalking);
_animVars.set("notIsTalking", !params.isTalking);
if (params.hipsEnabled) {
_animVars.set("hipsType", (int)IKTarget::Type::RotationAndPosition);
_animVars.set("hipsPosition", extractTranslation(params.hipsMatrix));
_animVars.set("hipsRotation", glmExtractRotation(params.hipsMatrix) * Quaternions::Y_180);
} else {
_animVars.set("hipsType", (int)IKTarget::Type::Unknown);
}
// by default this IK target is disabled.
_animVars.set("spine2Type", (int)IKTarget::Type::Unknown);
}
void Rig::updateFromEyeParameters(const EyeParameters& params) {
@ -1161,13 +1172,19 @@ void Rig::updateFromHandAndFeetParameters(const HandAndFeetParameters& params, f
const glm::vec3 bodyCapsuleStart = bodyCapsuleCenter - glm::vec3(0, params.bodyCapsuleHalfHeight, 0);
const glm::vec3 bodyCapsuleEnd = bodyCapsuleCenter + glm::vec3(0, params.bodyCapsuleHalfHeight, 0);
// TODO: add isHipsEnabled
bool bodySensorTrackingEnabled = params.isLeftFootEnabled || params.isRightFootEnabled;
if (params.isLeftEnabled) {
// prevent the hand IK targets from intersecting the body capsule
glm::vec3 handPosition = params.leftPosition;
glm::vec3 displacement;
if (findSphereCapsulePenetration(handPosition, HAND_RADIUS, bodyCapsuleStart, bodyCapsuleEnd, bodyCapsuleRadius, displacement)) {
handPosition -= displacement;
if (!bodySensorTrackingEnabled) {
// prevent the hand IK targets from intersecting the body capsule
glm::vec3 displacement;
if (findSphereCapsulePenetration(handPosition, HAND_RADIUS, bodyCapsuleStart, bodyCapsuleEnd, bodyCapsuleRadius, displacement)) {
handPosition -= displacement;
}
}
_animVars.set("leftHandPosition", handPosition);
@ -1181,11 +1198,14 @@ void Rig::updateFromHandAndFeetParameters(const HandAndFeetParameters& params, f
if (params.isRightEnabled) {
// prevent the hand IK targets from intersecting the body capsule
glm::vec3 handPosition = params.rightPosition;
glm::vec3 displacement;
if (findSphereCapsulePenetration(handPosition, HAND_RADIUS, bodyCapsuleStart, bodyCapsuleEnd, bodyCapsuleRadius, displacement)) {
handPosition -= displacement;
if (!bodySensorTrackingEnabled) {
// prevent the hand IK targets from intersecting the body capsule
glm::vec3 displacement;
if (findSphereCapsulePenetration(handPosition, HAND_RADIUS, bodyCapsuleStart, bodyCapsuleEnd, bodyCapsuleRadius, displacement)) {
handPosition -= displacement;
}
}
_animVars.set("rightHandPosition", handPosition);

2
libraries/animation/src/Rig.h
View file

@ -45,6 +45,8 @@ public:
glm::quat worldHeadOrientation = glm::quat(); // world space (-z forward)
glm::quat rigHeadOrientation = glm::quat(); // rig space (-z forward)
glm::vec3 rigHeadPosition = glm::vec3(); // rig space
glm::mat4 hipsMatrix = glm::mat4(); // rig space
bool hipsEnabled = false;
bool isInHMD = false;
int neckJointIndex = -1;
bool isTalking = false;

7
libraries/script-engine/src/Quat.cpp
View file

@ -122,3 +122,10 @@ bool Quat::equal(const glm::quat& q1, const glm::quat& q2) {
return q1 == q2;
}
glm::quat Quat::cancelOutRollAndPitch(const glm::quat& q) {
return ::cancelOutRollAndPitch(q);
}
glm::quat Quat::cancelOutRoll(const glm::quat& q) {
return ::cancelOutRoll(q);
}

2
libraries/script-engine/src/Quat.h
View file

@ -60,6 +60,8 @@ public slots:
float dot(const glm::quat& q1, const glm::quat& q2);
void print(const QString& label, const glm::quat& q);
bool equal(const glm::quat& q1, const glm::quat& q2);
glm::quat cancelOutRollAndPitch(const glm::quat& q);
glm::quat cancelOutRoll(const glm::quat& q);
};
#endif // hifi_Quat_h

118
scripts/developer/tests/hipsIkTest.js Normal file
View file

@ -0,0 +1,118 @@
//
// hipsIKTest.js
//
// Created by Anthony Thibault on 4/24/17
// Copyright 2017 High Fidelity, Inc.
//
// Test procedural manipulation of the Avatar hips IK target.
// Pull the left and right triggers on your hand controllers, you hips should begin to gyrate in an amusing mannor.
//
// Distributed under the Apache License, Version 2.0.
// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
/* global Xform */
Script.include("/~/system/libraries/Xform.js");
var calibrated = false;
var rightTriggerPressed = false;
var leftTriggerPressed = false;
var MAPPING_NAME = "com.highfidelity.hipsIkTest";
var mapping = Controller.newMapping(MAPPING_NAME);
mapping.from([Controller.Standard.RTClick]).peek().to(function (value) {
rightTriggerPressed = (value !== 0) ? true : false;
});
mapping.from([Controller.Standard.LTClick]).peek().to(function (value) {
leftTriggerPressed = (value !== 0) ? true : false;
});
Controller.enableMapping(MAPPING_NAME);
var ANIM_VARS = [
"headType",
"hipsType",
"hipsPosition",
"hipsRotation"
];
var ZERO = {x: 0, y: 0, z: 0};
var X_AXIS = {x: 1, y: 0, z: 0};
var Y_AXIS = {x: 0, y: 1, z: 0};
var Y_180 = {x: 0, y: 1, z: 0, w: 0};
var Y_180_XFORM = new Xform(Y_180, {x: 0, y: 0, z: 0});
var hips = undefined;
function computeCurrentXform(jointIndex) {
var currentXform = new Xform(MyAvatar.getAbsoluteJointRotationInObjectFrame(jointIndex),
MyAvatar.getAbsoluteJointTranslationInObjectFrame(jointIndex));
return Xform.mul(Y_180_XFORM, currentXform);
}
function calibrate() {
hips = computeCurrentXform(MyAvatar.getJointIndex("Hips"));
}
var ikTypes = {
RotationAndPosition: 0,
RotationOnly: 1,
HmdHead: 2,
HipsRelativeRotationAndPosition: 3,
Off: 4
};
function circleOffset(radius, theta, normal) {
var pos = {x: radius * Math.cos(theta), y: radius * Math.sin(theta), z: 0};
var lookAtRot = Quat.lookAt(normal, ZERO, X_AXIS);
return Vec3.multiplyQbyV(lookAtRot, pos);
}
var handlerId;
function update(dt) {
if (rightTriggerPressed && leftTriggerPressed) {
if (!calibrated) {
calibrate();
calibrated = true;
if (handlerId) {
MyAvatar.removeAnimationStateHandler(handlerId);
handlerId = undefined;
} else {
var n = Y_AXIS;
var t = 0;
handlerId = MyAvatar.addAnimationStateHandler(function (props) {
t += (1 / 60) * 4;
var result = {}, xform;
if (hips) {
xform = hips;
result.hipsType = ikTypes.RotationAndPosition;
result.hipsPosition = Vec3.sum(xform.pos, circleOffset(0.1, t, n));
result.hipsRotation = xform.rot;
result.headType = ikTypes.RotationAndPosition;
} else {
result.headType = ikTypes.HmdHead;
result.hipsType = props.hipsType;
result.hipsPosition = props.hipsPosition;
result.hipsRotation = props.hipsRotation;
}
return result;
}, ANIM_VARS);
}
}
} else {
calibrated = false;
}
}
Script.update.connect(update);
Script.scriptEnding.connect(function () {
Controller.disableMapping(MAPPING_NAME);
Script.update.disconnect(update);
});

307
scripts/developer/tests/viveMotionCapture.js Normal file
View file

@ -0,0 +1,307 @@
/* global Xform */
Script.include("/~/system/libraries/Xform.js");
var TRACKED_OBJECT_POSES = [
"TrackedObject00", "TrackedObject01", "TrackedObject02", "TrackedObject03",
"TrackedObject04", "TrackedObject05", "TrackedObject06", "TrackedObject07",
"TrackedObject08", "TrackedObject09", "TrackedObject10", "TrackedObject11",
"TrackedObject12", "TrackedObject13", "TrackedObject14", "TrackedObject15"
];
var triggerPressHandled = false;
var rightTriggerPressed = false;
var leftTriggerPressed = false;
var MAPPING_NAME = "com.highfidelity.viveMotionCapture";
var mapping = Controller.newMapping(MAPPING_NAME);
mapping.from([Controller.Standard.RTClick]).peek().to(function (value) {
rightTriggerPressed = (value !== 0) ? true : false;
});
mapping.from([Controller.Standard.LTClick]).peek().to(function (value) {
leftTriggerPressed = (value !== 0) ? true : false;
});
Controller.enableMapping(MAPPING_NAME);
var leftFoot;
var rightFoot;
var hips;
var spine2;
var FEET_ONLY = 0;
var FEET_AND_HIPS = 1;
var FEET_AND_CHEST = 2;
var FEET_HIPS_AND_CHEST = 3;
var AUTO = 4;
var SENSOR_CONFIG_NAMES = [
"FeetOnly",
"FeetAndHips",
"FeetAndChest",
"FeetHipsAndChest",
"Auto"
];
var sensorConfig = AUTO;
var Y_180 = {x: 0, y: 1, z: 0, w: 0};
var Y_180_XFORM = new Xform(Y_180, {x: 0, y: 0, z: 0});
function computeOffsetXform(defaultToReferenceXform, pose, jointIndex) {
var poseXform = new Xform(pose.rotation, pose.translation);
var defaultJointXform = new Xform(MyAvatar.getAbsoluteDefaultJointRotationInObjectFrame(jointIndex),
MyAvatar.getAbsoluteDefaultJointTranslationInObjectFrame(jointIndex));
var referenceJointXform = Xform.mul(defaultToReferenceXform, defaultJointXform);
return Xform.mul(poseXform.inv(), referenceJointXform);
}
function computeDefaultToReferenceXform() {
var headIndex = MyAvatar.getJointIndex("Head");
if (headIndex >= 0) {
var defaultHeadXform = new Xform(MyAvatar.getAbsoluteDefaultJointRotationInObjectFrame(headIndex),
MyAvatar.getAbsoluteDefaultJointTranslationInObjectFrame(headIndex));
var currentHeadXform = new Xform(Quat.cancelOutRollAndPitch(MyAvatar.getAbsoluteJointRotationInObjectFrame(headIndex)),
MyAvatar.getAbsoluteJointTranslationInObjectFrame(headIndex));
var defaultToReferenceXform = Xform.mul(currentHeadXform, defaultHeadXform.inv());
return defaultToReferenceXform;
} else {
return Xform.ident();
}
}
function calibrate() {
leftFoot = undefined;
rightFoot = undefined;
hips = undefined;
spine2 = undefined;
var defaultToReferenceXform = computeDefaultToReferenceXform();
var poses = [];
if (Controller.Hardware.Vive) {
TRACKED_OBJECT_POSES.forEach(function (key) {
var channel = Controller.Hardware.Vive[key];
var pose = Controller.getPoseValue(channel);
if (pose.valid) {
poses.push({
channel: channel,
pose: pose
});
}
});
}
print("AJT: calibrating, num tracked poses = " + poses.length + ", sensorConfig = " + SENSOR_CONFIG_NAMES[sensorConfig]);
var config = sensorConfig;
if (config === AUTO) {
if (poses.length === 2) {
config = FEET_ONLY;
} else if (poses.length === 3) {
config = FEET_AND_HIPS;
} else if (poses.length >= 4) {
config = FEET_HIPS_AND_CHEST;
} else {
print("AJT: auto config failed: poses.length = " + poses.length);
config = FEET_ONLY;
}
}
if (poses.length >= 2) {
// sort by y
poses.sort(function(a, b) {
var ay = a.pose.translation.y;
var by = b.pose.translation.y;
return ay - by;
});
if (poses[0].pose.translation.x > poses[1].pose.translation.x) {
rightFoot = poses[0];
leftFoot = poses[1];
} else {
rightFoot = poses[1];
leftFoot = poses[0];
}
// compute offsets
rightFoot.offsetXform = computeOffsetXform(defaultToReferenceXform, rightFoot.pose, MyAvatar.getJointIndex("RightFoot"));
leftFoot.offsetXform = computeOffsetXform(defaultToReferenceXform, leftFoot.pose, MyAvatar.getJointIndex("LeftFoot"));
print("AJT: rightFoot = " + JSON.stringify(rightFoot));
print("AJT: leftFoot = " + JSON.stringify(leftFoot));
if (config === FEET_ONLY) {
// we're done!
} else if (config === FEET_AND_HIPS && poses.length >= 3) {
hips = poses[2];
} else if (config === FEET_AND_CHEST && poses.length >= 3) {
spine2 = poses[2];
} else if (config === FEET_HIPS_AND_CHEST && poses.length >= 4) {
hips = poses[2];
spine2 = poses[3];
} else {
// TODO: better error messages
print("AJT: could not calibrate for sensor config " + SENSOR_CONFIG_NAMES[config] + ", please try again!");
}
if (hips) {
hips.offsetXform = computeOffsetXform(defaultToReferenceXform, hips.pose, MyAvatar.getJointIndex("Hips"));
print("AJT: hips = " + JSON.stringify(hips));
}
if (spine2) {
spine2.offsetXform = computeOffsetXform(defaultToReferenceXform, spine2.pose, MyAvatar.getJointIndex("Spine2"));
print("AJT: spine2 = " + JSON.stringify(spine2));
}
} else {
print("AJT: could not find two trackers, try again!");
}
}
var ikTypes = {
RotationAndPosition: 0,
RotationOnly: 1,
HmdHead: 2,
HipsRelativeRotationAndPosition: 3,
Off: 4
};
var handlerId;
function computeIKTargetXform(jointInfo) {
var pose = Controller.getPoseValue(jointInfo.channel);
var offsetXform = jointInfo.offsetXform;
return Xform.mul(Y_180_XFORM, Xform.mul(new Xform(pose.rotation, pose.translation), offsetXform));
}
function update(dt) {
if (rightTriggerPressed && leftTriggerPressed) {
if (!triggerPressHandled) {
triggerPressHandled = true;
if (handlerId) {
print("AJT: UN-CALIBRATE!");
// go back to normal, vive pucks will be ignored.
leftFoot = undefined;
rightFoot = undefined;
hips = undefined;
spine2 = undefined;
if (handlerId) {
print("AJT: un-hooking animation state handler");
MyAvatar.removeAnimationStateHandler(handlerId);
handlerId = undefined;
}
} else {
print("AJT: CALIBRATE!");
calibrate();
var animVars = [];
if (leftFoot) {
animVars.push("leftFootType");
animVars.push("leftFootPosition");
animVars.push("leftFootRotation");
}
if (rightFoot) {
animVars.push("rightFootType");
animVars.push("rightFootPosition");
animVars.push("rightFootRotation");
}
if (hips) {
animVars.push("hipsType");
animVars.push("hipsPosition");
animVars.push("hipsRotation");
}
if (spine2) {
animVars.push("spine2Type");
animVars.push("spine2Position");
animVars.push("spine2Rotation");
}
// hook up new anim state handler that maps vive pucks to ik system.
handlerId = MyAvatar.addAnimationStateHandler(function (props) {
var result = {}, xform;
if (rightFoot) {
xform = computeIKTargetXform(rightFoot);
result.rightFootType = ikTypes.RotationAndPosition;
result.rightFootPosition = xform.pos;
result.rightFootRotation = xform.rot;
}
if (leftFoot) {
xform = computeIKTargetXform(leftFoot);
result.leftFootType = ikTypes.RotationAndPosition;
result.leftFootPosition = xform.pos;
result.leftFootRotation = xform.rot;
}
if (hips) {
xform = computeIKTargetXform(hips);
result.hipsType = ikTypes.RotationAndPosition;
result.hipsPosition = xform.pos;
result.hipsRotation = xform.rot;
}
if (spine2) {
xform = computeIKTargetXform(spine2);
result.spine2Type = ikTypes.RotationAndPosition;
result.spine2Position = xform.pos;
result.spine2Rotation = xform.rot;
}
return result;
}, animVars);
}
}
} else {
triggerPressHandled = false;
}
var drawMarkers = false;
if (drawMarkers) {
var RED = {x: 1, y: 0, z: 0, w: 1};
var BLUE = {x: 0, y: 0, z: 1, w: 1};
if (leftFoot) {
var leftFootPose = Controller.getPoseValue(leftFoot.channel);
DebugDraw.addMyAvatarMarker("leftFootTracker", leftFootPose.rotation, leftFootPose.translation, BLUE);
}
if (rightFoot) {
var rightFootPose = Controller.getPoseValue(rightFoot.channel);
DebugDraw.addMyAvatarMarker("rightFootTracker", rightFootPose.rotation, rightFootPose.translation, RED);
}
if (hips) {
var hipsPose = Controller.getPoseValue(hips.channel);
DebugDraw.addMyAvatarMarker("hipsTracker", hipsPose.rotation, hipsPose.translation, GREEN);
}
}
var drawReferencePose = false;
if (drawReferencePose) {
var GREEN = {x: 0, y: 1, z: 0, w: 1};
var defaultToReferenceXform = computeDefaultToReferenceXform();
var leftFootIndex = MyAvatar.getJointIndex("LeftFoot");
if (leftFootIndex > 0) {
var defaultLeftFootXform = new Xform(MyAvatar.getAbsoluteDefaultJointRotationInObjectFrame(leftFootIndex),
MyAvatar.getAbsoluteDefaultJointTranslationInObjectFrame(leftFootIndex));
var referenceLeftFootXform = Xform.mul(defaultToReferenceXform, defaultLeftFootXform);
DebugDraw.addMyAvatarMarker("leftFootTracker", referenceLeftFootXform.rot, referenceLeftFootXform.pos, GREEN);
}
}
}
Script.update.connect(update);
Script.scriptEnding.connect(function () {
Controller.disableMapping(MAPPING_NAME);
Script.update.disconnect(update);
});

4
scripts/developer/tests/viveTrackedObjects.js
View file

@ -18,14 +18,14 @@ function shutdown() {
});
}
var WHITE = {x: 1, y: 1, z: 1, w: 1};
var BLUE = {x: 0, y: 0, z: 1, w: 1};
function update(dt) {
if (Controller.Hardware.Vive) {
TRACKED_OBJECT_POSES.forEach(function (key) {
var pose = Controller.getPoseValue(Controller.Hardware.Vive[key]);
if (pose.valid) {
DebugDraw.addMyAvatarMarker(key, pose.rotation, pose.translation, WHITE);
DebugDraw.addMyAvatarMarker(key, pose.rotation, pose.translation, BLUE);
} else {
DebugDraw.removeMyAvatarMarker(key);
}