From 53c439ffba3e4cc105d011b42ed207a4b6fb68c3 Mon Sep 17 00:00:00 2001
From: ZappoMan <brad@highfidelity.io>
Date: Thu, 30 Mar 2017 19:11:16 -0700
Subject: [PATCH 1/2] add foot IK targets as actions
---
interface/resources/controllers/kinect.json | 4 +-
interface/resources/controllers/standard.json | 5 +-
interface/src/Application.cpp | 4 +
interface/src/avatar/MyAvatar.cpp | 39 +++++++++
interface/src/avatar/MyAvatar.h | 11 +++
interface/src/avatar/SkeletonModel.cpp | 44 +++++++---
libraries/animation/src/Rig.cpp | 24 +++++-
libraries/animation/src/Rig.h | 11 ++-
.../controllers/src/controllers/Actions.cpp | 2 +
.../controllers/src/controllers/Actions.h | 2 +
.../src/controllers/StandardController.cpp | 2 +
.../controllers/proceduralFootPoseExample.js | 81 +++++++++++++++++++
script-archive/controllers/puppetFeet2.js | 35 ++++++++
13 files changed, 245 insertions(+), 19 deletions(-)
create mode 100644 script-archive/controllers/proceduralFootPoseExample.js
create mode 100644 script-archive/controllers/puppetFeet2.js
diff --git a/interface/resources/controllers/kinect.json b/interface/resources/controllers/kinect.json
index 96a504030c..eb1d301af9 100644
--- a/interface/resources/controllers/kinect.json
+++ b/interface/resources/controllers/kinect.json
@@ -2,6 +2,8 @@
"name": "Kinect to Standard",
"channels": [
{ "from": "Kinect.LeftHand", "to": "Standard.LeftHand" },
- { "from": "Kinect.RightHand", "to": "Standard.RightHand" }
+ { "from": "Kinect.RightHand", "to": "Standard.RightHand" },
+ { "from": "Kinect.LeftFoot", "to": "Standard.LeftFoot" },
+ { "from": "Kinect.RightFoot", "to": "Standard.RightFoot" }
]
}
diff --git a/interface/resources/controllers/standard.json b/interface/resources/controllers/standard.json
index 9e3b2f4d13..53285ea974 100644
--- a/interface/resources/controllers/standard.json
+++ b/interface/resources/controllers/standard.json
@@ -58,6 +58,9 @@
{ "from": "Standard.RT", "to": "Actions.RightHandClick" },
{ "from": "Standard.LeftHand", "to": "Actions.LeftHand" },
- { "from": "Standard.RightHand", "to": "Actions.RightHand" }
+ { "from": "Standard.RightHand", "to": "Actions.RightHand" },
+
+ { "from": "Standard.LeftFoot", "to": "Actions.LeftFoot" },
+ { "from": "Standard.RightFoot", "to": "Actions.RightFoot" }
]
}
diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp
index 25a585ea34..a1a4a63e5b 100644
--- a/interface/src/Application.cpp
+++ b/interface/src/Application.cpp
@@ -4386,6 +4386,10 @@ void Application::update(float deltaTime) {
auto avatarToSensorMatrix = worldToSensorMatrix * myAvatarMatrix;
myAvatar->setHandControllerPosesInSensorFrame(leftHandPose.transform(avatarToSensorMatrix), rightHandPose.transform(avatarToSensorMatrix));
+ controller::Pose leftFootPose = userInputMapper->getPoseState(controller::Action::LEFT_FOOT);
+ controller::Pose rightFootPose = userInputMapper->getPoseState(controller::Action::RIGHT_FOOT);
+ myAvatar->setFootControllerPosesInSensorFrame(leftFootPose.transform(avatarToSensorMatrix), rightFootPose.transform(avatarToSensorMatrix));
+
updateThreads(deltaTime); // If running non-threaded, then give the threads some time to process...
updateDialogs(deltaTime); // update various stats dialogs if present
diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index 0d0390a365..d1edf9d44e 100644
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -1345,6 +1345,45 @@ controller::Pose MyAvatar::getRightHandControllerPoseInAvatarFrame() const {
return getRightHandControllerPoseInWorldFrame().transform(invAvatarMatrix);
}
+void MyAvatar::setFootControllerPosesInSensorFrame(const controller::Pose& left, const controller::Pose& right) {
+ if (controller::InputDevice::getLowVelocityFilter()) {
+ auto oldLeftPose = getLeftFootControllerPoseInSensorFrame();
+ auto oldRightPose = getRightFootControllerPoseInSensorFrame();
+ _leftFootControllerPoseInSensorFrameCache.set(applyLowVelocityFilter(oldLeftPose, left));
+ _rightFootControllerPoseInSensorFrameCache.set(applyLowVelocityFilter(oldRightPose, right));
+ } else {
+ _leftFootControllerPoseInSensorFrameCache.set(left);
+ _rightFootControllerPoseInSensorFrameCache.set(right);
+ }
+}
+
+controller::Pose MyAvatar::getLeftFootControllerPoseInSensorFrame() const {
+ return _leftFootControllerPoseInSensorFrameCache.get();
+}
+
+controller::Pose MyAvatar::getRightFootControllerPoseInSensorFrame() const {
+ return _rightFootControllerPoseInSensorFrameCache.get();
+}
+
+controller::Pose MyAvatar::getLeftFootControllerPoseInWorldFrame() const {
+ return _leftFootControllerPoseInSensorFrameCache.get().transform(getSensorToWorldMatrix());
+}
+
+controller::Pose MyAvatar::getRightFootControllerPoseInWorldFrame() const {
+ return _rightFootControllerPoseInSensorFrameCache.get().transform(getSensorToWorldMatrix());
+}
+
+controller::Pose MyAvatar::getLeftFootControllerPoseInAvatarFrame() const {
+ glm::mat4 invAvatarMatrix = glm::inverse(createMatFromQuatAndPos(getOrientation(), getPosition()));
+ return getLeftFootControllerPoseInWorldFrame().transform(invAvatarMatrix);
+}
+
+controller::Pose MyAvatar::getRightFootControllerPoseInAvatarFrame() const {
+ glm::mat4 invAvatarMatrix = glm::inverse(createMatFromQuatAndPos(getOrientation(), getPosition()));
+ return getRightFootControllerPoseInWorldFrame().transform(invAvatarMatrix);
+}
+
+
void MyAvatar::updateMotors() {
_characterController.clearMotors();
glm::quat motorRotation;
diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h
index add57cf5be..097d3a1059 100644
--- a/interface/src/avatar/MyAvatar.h
+++ b/interface/src/avatar/MyAvatar.h
@@ -445,6 +445,14 @@ public:
controller::Pose getLeftHandControllerPoseInAvatarFrame() const;
controller::Pose getRightHandControllerPoseInAvatarFrame() const;
+ void setFootControllerPosesInSensorFrame(const controller::Pose& left, const controller::Pose& right);
+ controller::Pose getLeftFootControllerPoseInSensorFrame() const;
+ controller::Pose getRightFootControllerPoseInSensorFrame() const;
+ controller::Pose getLeftFootControllerPoseInWorldFrame() const;
+ controller::Pose getRightFootControllerPoseInWorldFrame() const;
+ controller::Pose getLeftFootControllerPoseInAvatarFrame() const;
+ controller::Pose getRightFootControllerPoseInAvatarFrame() const;
+
bool hasDriveInput() const;
Q_INVOKABLE void setCharacterControllerEnabled(bool enabled);
@@ -684,6 +692,9 @@ private:
ThreadSafeValueCache<controller::Pose> _leftHandControllerPoseInSensorFrameCache { controller::Pose() };
ThreadSafeValueCache<controller::Pose> _rightHandControllerPoseInSensorFrameCache { controller::Pose() };
+ ThreadSafeValueCache<controller::Pose> _leftFootControllerPoseInSensorFrameCache{ controller::Pose() };
+ ThreadSafeValueCache<controller::Pose> _rightFootControllerPoseInSensorFrameCache{ controller::Pose() };
+
bool _hmdLeanRecenterEnabled = true;
AnimPose _prePhysicsRoomPose;
diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp
index 476abf8d4b..d7dd93cedf 100644
--- a/interface/src/avatar/SkeletonModel.cpp
+++ b/interface/src/avatar/SkeletonModel.cpp
@@ -132,31 +132,49 @@ void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) {
_rig->updateFromHeadParameters(headParams, deltaTime);
- Rig::HandParameters handParams;
+ Rig::HandAndFeetParameters handAndFeetParams;
auto leftPose = myAvatar->getLeftHandControllerPoseInAvatarFrame();
if (leftPose.isValid()) {
- handParams.isLeftEnabled = true;
- handParams.leftPosition = Quaternions::Y_180 * leftPose.getTranslation();
- handParams.leftOrientation = Quaternions::Y_180 * leftPose.getRotation();
+ handAndFeetParams.isLeftEnabled = true;
+ handAndFeetParams.leftPosition = Quaternions::Y_180 * leftPose.getTranslation();
+ handAndFeetParams.leftOrientation = Quaternions::Y_180 * leftPose.getRotation();
} else {
- handParams.isLeftEnabled = false;
+ handAndFeetParams.isLeftEnabled = false;
}
auto rightPose = myAvatar->getRightHandControllerPoseInAvatarFrame();
if (rightPose.isValid()) {
- handParams.isRightEnabled = true;
- handParams.rightPosition = Quaternions::Y_180 * rightPose.getTranslation();
- handParams.rightOrientation = Quaternions::Y_180 * rightPose.getRotation();
+ handAndFeetParams.isRightEnabled = true;
+ handAndFeetParams.rightPosition = Quaternions::Y_180 * rightPose.getTranslation();
+ handAndFeetParams.rightOrientation = Quaternions::Y_180 * rightPose.getRotation();
} else {
- handParams.isRightEnabled = false;
+ handAndFeetParams.isRightEnabled = false;
}
- handParams.bodyCapsuleRadius = myAvatar->getCharacterController()->getCapsuleRadius();
- handParams.bodyCapsuleHalfHeight = myAvatar->getCharacterController()->getCapsuleHalfHeight();
- handParams.bodyCapsuleLocalOffset = myAvatar->getCharacterController()->getCapsuleLocalOffset();
+ auto leftFootPose = myAvatar->getLeftFootControllerPoseInAvatarFrame();
+ if (leftFootPose.isValid()) {
+ handAndFeetParams.isLeftFootEnabled = true;
+ handAndFeetParams.leftFootPosition = Quaternions::Y_180 * leftFootPose.getTranslation();
+ handAndFeetParams.leftFootOrientation = Quaternions::Y_180 * leftFootPose.getRotation();
+ } else {
+ handAndFeetParams.isLeftFootEnabled = false;
+ }
- _rig->updateFromHandParameters(handParams, deltaTime);
+ auto rightFootPose = myAvatar->getRightFootControllerPoseInAvatarFrame();
+ if (rightFootPose.isValid()) {
+ handAndFeetParams.isRightFootEnabled = true;
+ handAndFeetParams.rightFootPosition = Quaternions::Y_180 * rightFootPose.getTranslation();
+ handAndFeetParams.rightFootOrientation = Quaternions::Y_180 * rightFootPose.getRotation();
+ } else {
+ handAndFeetParams.isRightFootEnabled = false;
+ }
+
+ handAndFeetParams.bodyCapsuleRadius = myAvatar->getCharacterController()->getCapsuleRadius();
+ handAndFeetParams.bodyCapsuleHalfHeight = myAvatar->getCharacterController()->getCapsuleHalfHeight();
+ handAndFeetParams.bodyCapsuleLocalOffset = myAvatar->getCharacterController()->getCapsuleLocalOffset();
+
+ _rig->updateFromHandAndFeetParameters(handAndFeetParams, deltaTime);
Rig::CharacterControllerState ccState = convertCharacterControllerState(myAvatar->getCharacterController()->getState());
diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp
index 8b54f0ee92..020ea1d657 100644
--- a/libraries/animation/src/Rig.cpp
+++ b/libraries/animation/src/Rig.cpp
@@ -1146,9 +1146,8 @@ void Rig::updateEyeJoint(int index, const glm::vec3& modelTranslation, const glm
}
}
-void Rig::updateFromHandParameters(const HandParameters& params, float dt) {
+void Rig::updateFromHandAndFeetParameters(const HandAndFeetParameters& params, float dt) {
if (_animSkeleton && _animNode) {
-
const float HAND_RADIUS = 0.05f;
int hipsIndex = indexOfJoint("Hips");
glm::vec3 hipsTrans;
@@ -1197,6 +1196,27 @@ void Rig::updateFromHandParameters(const HandParameters& params, float dt) {
_animVars.unset("rightHandRotation");
_animVars.set("rightHandType", (int)IKTarget::Type::HipsRelativeRotationAndPosition);
}
+
+ if (params.isLeftFootEnabled) {
+ _animVars.set("leftFootPosition", params.leftFootPosition);
+ _animVars.set("leftFootRotation", params.leftFootOrientation);
+ _animVars.set("leftFootType", (int)IKTarget::Type::RotationAndPosition);
+ } else {
+ _animVars.unset("leftFootPosition");
+ _animVars.unset("leftFootRotation");
+ _animVars.set("leftFootType", (int)IKTarget::Type::HipsRelativeRotationAndPosition);
+ }
+
+ if (params.isRightFootEnabled) {
+ _animVars.set("rightFootPosition", params.rightFootPosition);
+ _animVars.set("rightFootRotation", params.rightFootOrientation);
+ _animVars.set("rightFootType", (int)IKTarget::Type::RotationAndPosition);
+ } else {
+ _animVars.unset("rightFootPosition");
+ _animVars.unset("rightFootRotation");
+ _animVars.set("rightFootType", (int)IKTarget::Type::HipsRelativeRotationAndPosition);
+ }
+
}
}
diff --git a/libraries/animation/src/Rig.h b/libraries/animation/src/Rig.h
index ea498e6a69..2cd20c2704 100644
--- a/libraries/animation/src/Rig.h
+++ b/libraries/animation/src/Rig.h
@@ -60,7 +60,7 @@ public:
int rightEyeJointIndex = -1;
};
- struct HandParameters {
+ struct HandAndFeetParameters {
bool isLeftEnabled;
bool isRightEnabled;
float bodyCapsuleRadius;
@@ -70,6 +70,13 @@ public:
glm::quat leftOrientation = glm::quat(); // rig space (z forward)
glm::vec3 rightPosition = glm::vec3(); // rig space
glm::quat rightOrientation = glm::quat(); // rig space (z forward)
+
+ bool isLeftFootEnabled;
+ bool isRightFootEnabled;
+ glm::vec3 leftFootPosition = glm::vec3(); // rig space
+ glm::quat leftFootOrientation = glm::quat(); // rig space (z forward)
+ glm::vec3 rightFootPosition = glm::vec3(); // rig space
+ glm::quat rightFootOrientation = glm::quat(); // rig space (z forward)
};
enum class CharacterControllerState {
@@ -185,7 +192,7 @@ public:
void updateFromHeadParameters(const HeadParameters& params, float dt);
void updateFromEyeParameters(const EyeParameters& params);
- void updateFromHandParameters(const HandParameters& params, float dt);
+ void updateFromHandAndFeetParameters(const HandAndFeetParameters& params, float dt);
void initAnimGraph(const QUrl& url);
diff --git a/libraries/controllers/src/controllers/Actions.cpp b/libraries/controllers/src/controllers/Actions.cpp
index 79ff4ecbf8..300fa684d9 100644
--- a/libraries/controllers/src/controllers/Actions.cpp
+++ b/libraries/controllers/src/controllers/Actions.cpp
@@ -51,6 +51,8 @@ namespace controller {
makePosePair(Action::LEFT_HAND, "LeftHand"),
makePosePair(Action::RIGHT_HAND, "RightHand"),
+ makePosePair(Action::LEFT_FOOT, "LeftFoot"),
+ makePosePair(Action::RIGHT_FOOT, "RightFoot"),
makeButtonPair(Action::LEFT_HAND_CLICK, "LeftHandClick"),
makeButtonPair(Action::RIGHT_HAND_CLICK, "RightHandClick"),
diff --git a/libraries/controllers/src/controllers/Actions.h b/libraries/controllers/src/controllers/Actions.h
index 724d17d951..edf3dee07a 100644
--- a/libraries/controllers/src/controllers/Actions.h
+++ b/libraries/controllers/src/controllers/Actions.h
@@ -42,6 +42,8 @@ enum class Action {
LEFT_HAND = NUM_COMBINED_AXES,
RIGHT_HAND,
+ LEFT_FOOT,
+ RIGHT_FOOT,
LEFT_HAND_CLICK,
RIGHT_HAND_CLICK,
diff --git a/libraries/controllers/src/controllers/StandardController.cpp b/libraries/controllers/src/controllers/StandardController.cpp
index 02ae5706b7..cc90ee7b49 100644
--- a/libraries/controllers/src/controllers/StandardController.cpp
+++ b/libraries/controllers/src/controllers/StandardController.cpp
@@ -102,6 +102,8 @@ Input::NamedVector StandardController::getAvailableInputs() const {
// Poses
makePair(LEFT_HAND, "LeftHand"),
makePair(RIGHT_HAND, "RightHand"),
+ makePair(LEFT_FOOT, "LeftFoot"),
+ makePair(RIGHT_FOOT, "RightFoot"),
// Aliases, PlayStation style names
makePair(LB, "L1"),
diff --git a/script-archive/controllers/proceduralFootPoseExample.js b/script-archive/controllers/proceduralFootPoseExample.js
new file mode 100644
index 0000000000..e2504de04d
--- /dev/null
+++ b/script-archive/controllers/proceduralFootPoseExample.js
@@ -0,0 +1,81 @@
+//
+// proceduralFootPoseExample.js
+// examples/controllers
+//
+// Created by Brad Hefta-Gaub on 2015/12/15
+// Copyright 2015 High Fidelity, Inc.
+//
+// Distributed under the Apache License, Version 2.0.
+// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
+//
+
+
+var MAPPING_NAME = "com.highfidelity.examples.proceduralFootPose";
+var mapping = Controller.newMapping(MAPPING_NAME);
+var translation = { x: 0, y: 0.1, z: 0 };
+var translationDx = 0.01;
+var translationDy = 0.01;
+var translationDz = -0.01;
+var TRANSLATION_LIMIT = 0.5;
+
+var pitch = 45;
+var yaw = 0;
+var roll = 45;
+var pitchDelta = 1;
+var yawDelta = -1;
+var rollDelta = 1;
+var ROTATION_MIN = -90;
+var ROTATION_MAX = 90;
+
+mapping.from(function() {
+
+ // adjust the hand translation in a periodic back and forth motion for each of the 3 axes
+ translation.x = translation.x + translationDx;
+ translation.y = translation.y + translationDy;
+ translation.z = translation.z + translationDz;
+ if ((translation.x > TRANSLATION_LIMIT) || (translation.x < (-1 * TRANSLATION_LIMIT))) {
+ translationDx = translationDx * -1;
+ }
+ if ((translation.y > TRANSLATION_LIMIT) || (translation.y < (-1 * TRANSLATION_LIMIT))) {
+ translationDy = translationDy * -1;
+ }
+ if ((translation.z > TRANSLATION_LIMIT) || (translation.z < (-1 * TRANSLATION_LIMIT))) {
+ translationDz = translationDz * -1;
+ }
+
+ // adjust the hand rotation in a periodic back and forth motion for each of pitch/yaw/roll
+ pitch = pitch + pitchDelta;
+ yaw = yaw + yawDelta;
+ roll = roll + rollDelta;
+ if ((pitch > ROTATION_MAX) || (pitch < ROTATION_MIN)) {
+ pitchDelta = pitchDelta * -1;
+ }
+ if ((yaw > ROTATION_MAX) || (yaw < ROTATION_MIN)) {
+ yawDelta = yawDelta * -1;
+ }
+ if ((roll > ROTATION_MAX) || (roll < ROTATION_MIN)) {
+ rollDelta = rollDelta * -1;
+ }
+
+ var rotation = Quat.fromPitchYawRollDegrees(pitch, yaw, roll);
+
+ var pose = {
+ translation: translation,
+ rotation: rotation,
+ velocity: { x: 0, y: 0, z: 0 },
+ angularVelocity: { x: 0, y: 0, z: 0 }
+ };
+
+ Vec3.print("foot translation:", translation);
+ return pose;
+}).debug(true).to(Controller.Standard.LeftFoot);
+
+//mapping.from(Controller.Standard.LeftFoot).debug(true).to(Controller.Actions.LeftFoot);
+
+
+Controller.enableMapping(MAPPING_NAME);
+
+
+Script.scriptEnding.connect(function(){
+ mapping.disable();
+});
diff --git a/script-archive/controllers/puppetFeet2.js b/script-archive/controllers/puppetFeet2.js
new file mode 100644
index 0000000000..754f3c4270
--- /dev/null
+++ b/script-archive/controllers/puppetFeet2.js
@@ -0,0 +1,35 @@
+//
+// proceduralFootPoseExample.js
+// examples/controllers
+//
+// Created by Brad Hefta-Gaub on 2015/12/15
+// Copyright 2015 High Fidelity, Inc.
+//
+// Distributed under the Apache License, Version 2.0.
+// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
+//
+
+
+var MAPPING_NAME = "com.highfidelity.examples.proceduralFootPose";
+var mapping = Controller.newMapping(MAPPING_NAME);
+var puppetOffset = { x: 0, y: -1, z: 0 };
+
+mapping.from(function() {
+ var leftHandPose = Controller.getPoseValue(Controller.Standard.LeftHand);
+
+ var pose = {
+ translation: Vec3.sum(leftHandPose.translation, puppetOffset),
+ rotation: { x: 0, y: 0, z: 0, w: 0 }, //leftHandPose.rotation,
+ velocity: { x: 0, y: 0, z: 0 },
+ angularVelocity: { x: 0, y: 0, z: 0 }
+ };
+ return pose;
+}).to(Controller.Standard.LeftFoot);
+
+
+Controller.enableMapping(MAPPING_NAME);
+
+
+Script.scriptEnding.connect(function(){
+ mapping.disable();
+});
From 4da8e5e18f3d211637d508422d08dddf09b0d4ea Mon Sep 17 00:00:00 2001
From: ZappoMan <brad@highfidelity.io>
Date: Thu, 30 Mar 2017 20:01:59 -0700
Subject: [PATCH 2/2] fix left/right legs for kinect
---
plugins/hifiKinect/src/KinectPlugin.cpp | 11 +++++------
1 file changed, 5 insertions(+), 6 deletions(-)
diff --git a/plugins/hifiKinect/src/KinectPlugin.cpp b/plugins/hifiKinect/src/KinectPlugin.cpp
index 72c7926ec1..6d29a261dd 100644
--- a/plugins/hifiKinect/src/KinectPlugin.cpp
+++ b/plugins/hifiKinect/src/KinectPlugin.cpp
@@ -113,16 +113,16 @@ static controller::StandardPoseChannel KinectJointIndexToPoseIndexMap[KinectJoin
controller::RIGHT_FORE_ARM,
controller::RIGHT_HAND,
- controller::RIGHT_UP_LEG, // hip socket
- controller::RIGHT_LEG, // knee?
- controller::RIGHT_FOOT, // ankle?
- UNKNOWN_JOINT, // ????
-
controller::LEFT_UP_LEG, // hip socket
controller::LEFT_LEG, // knee?
controller::LEFT_FOOT, // ankle?
UNKNOWN_JOINT, // ????
+ controller::RIGHT_UP_LEG, // hip socket
+ controller::RIGHT_LEG, // knee?
+ controller::RIGHT_FOOT, // ankle?
+ UNKNOWN_JOINT, // ????
+
UNKNOWN_JOINT, /* SpineShoulder */
controller::LEFT_HAND_INDEX4,
@@ -130,7 +130,6 @@ static controller::StandardPoseChannel KinectJointIndexToPoseIndexMap[KinectJoin
controller::RIGHT_HAND_INDEX4,
controller::RIGHT_HAND_THUMB4,
-
};
// in rig frame