From c4f9a4805c9c1221a63bf79f2c504b9d38d094fd Mon Sep 17 00:00:00 2001
From: Anthony Thibault <ajt@hyperlogic.org>
Date: Sun, 7 Feb 2016 14:07:40 -0800
Subject: [PATCH 1/7] MyAvatar: moving platform support

* pass sum of desired and parent velocity onto avatar's character controller.
* pass local position and velocity to Rig for animation purposes.
---
 interface/src/avatar/MyAvatar.cpp        | 13 ++++++++++++-
 interface/src/avatar/SkeletonModel.cpp   | 22 +++++++++++++++++++++-
 libraries/shared/src/SpatiallyNestable.h |  3 ++-
 3 files changed, 35 insertions(+), 3 deletions(-)

diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index a2c74603ed..c7b415ba09 100644
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -1078,7 +1078,18 @@ void MyAvatar::rebuildCollisionShape() {
 
 void MyAvatar::prepareForPhysicsSimulation() {
     relayDriveKeysToCharacterController();
-    _characterController.setTargetVelocity(getTargetVelocity());
+
+    glm::vec3 parentVelocity;
+    bool success;
+    auto parentPtr = getParentPointer(success);
+    if (success && parentPtr && parentPtr->getNestableType() == NestableType::Entity) {
+        auto entityPtr = std::dynamic_pointer_cast<EntityItem>(parentPtr);
+        if (entityPtr) {
+            parentVelocity = entityPtr->getVelocity();
+        }
+    }
+
+    _characterController.setTargetVelocity(getTargetVelocity() + parentVelocity);
     _characterController.setPositionAndOrientation(getPosition(), getOrientation());
     if (qApp->isHMDMode()) {
         _follow.prePhysicsUpdate(*this, deriveBodyFromHMDSensor(), _bodySensorMatrix);
diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp
index 28ae27d11f..a942830f68 100644
--- a/interface/src/avatar/SkeletonModel.cpp
+++ b/interface/src/avatar/SkeletonModel.cpp
@@ -148,7 +148,27 @@ void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) {
         _rig->updateFromHandParameters(handParams, deltaTime);
 
         Rig::CharacterControllerState ccState = convertCharacterControllerState(myAvatar->getCharacterController()->getState());
-        _rig->computeMotionAnimationState(deltaTime, _owningAvatar->getPosition(), _owningAvatar->getVelocity(), _owningAvatar->getOrientation(), ccState);
+
+        auto velocity = myAvatar->getVelocity();
+        auto position = myAvatar->getPosition();
+        auto orientation = myAvatar->getOrientation();
+
+        // check to see if we are attached to an Entity.
+        bool success;
+        auto parentPtr = myAvatar->getParentPointer(success);
+        if (success && parentPtr && parentPtr->getNestableType() == NestableType::Entity) {
+            auto entityPtr = std::dynamic_pointer_cast<EntityItem>(parentPtr);
+            if (entityPtr) {
+                // TODO: Do we need account for angularVelocity of entity?
+
+                auto invParentRot = glm::inverse(entityPtr->getOrientation());
+                velocity = invParentRot * (myAvatar->getVelocity() - entityPtr->getVelocity());
+                position = myAvatar->getLocalPosition();
+                orientation = myAvatar->getLocalOrientation();
+            }
+        }
+
+        _rig->computeMotionAnimationState(deltaTime, position, velocity, orientation, ccState);
 
         // evaluate AnimGraph animation and update jointStates.
         Model::updateRig(deltaTime, parentTransform);
diff --git a/libraries/shared/src/SpatiallyNestable.h b/libraries/shared/src/SpatiallyNestable.h
index 2a0b58f368..8153316ed9 100644
--- a/libraries/shared/src/SpatiallyNestable.h
+++ b/libraries/shared/src/SpatiallyNestable.h
@@ -119,13 +119,14 @@ public:
     bool isDead() const { return _isDead; }
 
     bool isParentIDValid() const { bool success = false; getParentPointer(success); return success; }
+    SpatiallyNestablePointer getParentPointer(bool& success) const;
 
 protected:
     const NestableType _nestableType; // EntityItem or an AvatarData
     QUuid _id;
     QUuid _parentID; // what is this thing's transform relative to?
     quint16 _parentJointIndex { 0 }; // which joint of the parent is this relative to?
-    SpatiallyNestablePointer getParentPointer(bool& success) const;
+
     mutable SpatiallyNestableWeakPointer _parent;
 
     virtual void beParentOfChild(SpatiallyNestablePointer newChild) const;

From 20a38f613efc4c3c9645a9da36a41a88baf8719f Mon Sep 17 00:00:00 2001
From: "Anthony J. Thibault" <tony@highfidelity.io>
Date: Mon, 8 Feb 2016 14:30:28 -0800
Subject: [PATCH 2/7] Rig: remove position delta based velocity.

In general, the physics body velocity is MUCH more behaved now.
---
 libraries/animation/src/Rig.cpp | 28 ++--------------------------
 1 file changed, 2 insertions(+), 26 deletions(-)

diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp
index 7034d0f253..7b788fc2c3 100644
--- a/libraries/animation/src/Rig.cpp
+++ b/libraries/animation/src/Rig.cpp
@@ -507,32 +507,7 @@ static const std::vector<float> LATERAL_SPEEDS = { 0.2f, 0.65f }; // m/s
 void Rig::computeMotionAnimationState(float deltaTime, const glm::vec3& worldPosition, const glm::vec3& worldVelocity, const glm::quat& worldRotation, CharacterControllerState ccState) {
 
     glm::vec3 front = worldRotation * IDENTITY_FRONT;
-
-    // It can be more accurate/smooth to use velocity rather than position,
-    // but some modes (e.g., hmd standing) update position without updating velocity.
-    // It's very hard to debug hmd standing. (Look down at yourself, or have a second person observe. HMD third person is a bit undefined...)
-    // So, let's create our own workingVelocity from the worldPosition...
-    glm::vec3 workingVelocity = _lastVelocity;
-    glm::vec3 positionDelta = worldPosition - _lastPosition;
-
-    // Don't trust position delta if deltaTime is 'small'.
-    // NOTE: This is mostly just a work around for an issue in oculus 0.7 runtime, where
-    // Application::idle() is being called more frequently and with smaller dt's then expected.
-    const float SMALL_DELTA_TIME = 0.006f;  // 6 ms
-    if (deltaTime > SMALL_DELTA_TIME) {
-        workingVelocity = positionDelta / deltaTime;
-    }
-
-#if !WANT_DEBUG
-    // But for smoothest (non-hmd standing) results, go ahead and use velocity:
-    if (!positionDelta.x && !positionDelta.y && !positionDelta.z) {
-        workingVelocity = worldVelocity;
-    }
-#endif
-
-    if (deltaTime > SMALL_DELTA_TIME) {
-        _lastVelocity = workingVelocity;
-    }
+    glm::vec3 workingVelocity = worldVelocity;
 
     {
         glm::vec3 localVel = glm::inverse(worldRotation) * workingVelocity;
@@ -825,6 +800,7 @@ void Rig::computeMotionAnimationState(float deltaTime, const glm::vec3& worldPos
 
     _lastFront = front;
     _lastPosition = worldPosition;
+    _lastVelocity = workingVelocity;
 }
 
 // Allow script to add/remove handlers and report results, from within their thread.

From 97bcc543605eb343d7d71c47545a3c05a14dcd9c Mon Sep 17 00:00:00 2001
From: "Anthony J. Thibault" <tony@highfidelity.io>
Date: Mon, 8 Feb 2016 16:01:36 -0800
Subject: [PATCH 3/7] CharacterController: separate target velocity from parent
 velocity.

Also, disable damping on the rigidBody used by the CharacterController.
---
 examples/moving-platform.js                   | 41 +++++++++++++++++++
 interface/src/avatar/MyAvatar.cpp             |  3 +-
 .../src/avatar/MyCharacterController.cpp      |  1 +
 libraries/physics/src/CharacterController.cpp | 15 ++++---
 libraries/physics/src/CharacterController.h   |  4 +-
 5 files changed, 56 insertions(+), 8 deletions(-)
 create mode 100644 examples/moving-platform.js

diff --git a/examples/moving-platform.js b/examples/moving-platform.js
new file mode 100644
index 0000000000..94e45bb571
--- /dev/null
+++ b/examples/moving-platform.js
@@ -0,0 +1,41 @@
+var platform;
+
+function init() {
+    platform = Entities.addEntity({
+        name: "platform",
+        type: "Box",
+        position: { x: 0, y: 0, z: 0 },
+        dimensions: { x: 10, y: 2, z: 10 },
+        color: { red: 0, green: 0, blue: 255 },
+        gravity: { x: 0, y: 0, z: 0 },
+        visible: true,
+        locked: false,
+        lifetime: 6000,
+        velocity: { x: 1.0, y: 0, z: 0 },
+        damping: 0,
+        isDynamic: false
+    });
+    if (platform) {
+        MyAvatar.position = { x: 0, y: 3, z: 0 };
+        if (MyAvatar.getParentID() != platform) {
+            MyAvatar.setParentID(platform);
+        }
+    }
+}
+
+function update(dt) {
+
+}
+
+function shutdown() {
+    if (platform) {
+        MyAvatar.setParentID(0);
+        Entities.deleteEntity(platform);
+    }
+}
+
+
+init();
+
+Script.update.connect(update);
+Script.scriptEnding.connect(shutdown);
diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index c7b415ba09..1436c192b5 100644
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -1089,7 +1089,8 @@ void MyAvatar::prepareForPhysicsSimulation() {
         }
     }
 
-    _characterController.setTargetVelocity(getTargetVelocity() + parentVelocity);
+    _characterController.setParentVelocity(parentVelocity);
+    _characterController.setTargetVelocity(getTargetVelocity());
     _characterController.setPositionAndOrientation(getPosition(), getOrientation());
     if (qApp->isHMDMode()) {
         _follow.prePhysicsUpdate(*this, deriveBodyFromHMDSensor(), _bodySensorMatrix);
diff --git a/interface/src/avatar/MyCharacterController.cpp b/interface/src/avatar/MyCharacterController.cpp
index 07156e9294..ee77859337 100644
--- a/interface/src/avatar/MyCharacterController.cpp
+++ b/interface/src/avatar/MyCharacterController.cpp
@@ -67,6 +67,7 @@ void MyCharacterController::updateShapeIfNecessary() {
             _rigidBody->setAngularFactor(0.0f);
             _rigidBody->setWorldTransform(btTransform(glmToBullet(_avatar->getOrientation()),
                                                       glmToBullet(_avatar->getPosition())));
+            _rigidBody->setDamping(0.0f, 0.0f);
             if (_state == State::Hover) {
                 _rigidBody->setGravity(btVector3(0.0f, 0.0f, 0.0f));
             } else {
diff --git a/libraries/physics/src/CharacterController.cpp b/libraries/physics/src/CharacterController.cpp
index 1f61bb9a59..9dd40a35a7 100644
--- a/libraries/physics/src/CharacterController.cpp
+++ b/libraries/physics/src/CharacterController.cpp
@@ -54,7 +54,7 @@ CharacterController::CharacterController() {
 
     _floorDistance = MAX_FALL_HEIGHT;
 
-    _walkVelocity.setValue(0.0f, 0.0f, 0.0f);
+    _targetVelocity.setValue(0.0f, 0.0f, 0.0f);
     _followDesiredBodyTransform.setIdentity();
     _followTimeRemaining = 0.0f;
     _jumpSpeed = JUMP_SPEED;
@@ -166,12 +166,12 @@ void CharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) {
 
     const btScalar MIN_SPEED = 0.001f;
 
-    btVector3 actualVelocity = _rigidBody->getLinearVelocity();
+    btVector3 actualVelocity = _rigidBody->getLinearVelocity() - _parentVelocity;
     if (actualVelocity.length() < MIN_SPEED) {
         actualVelocity = btVector3(0.0f, 0.0f, 0.0f);
     }
 
-    btVector3 desiredVelocity = _walkVelocity;
+    btVector3 desiredVelocity = _targetVelocity;
     if (desiredVelocity.length() < MIN_SPEED) {
         desiredVelocity = btVector3(0.0f, 0.0f, 0.0f);
     }
@@ -212,7 +212,7 @@ void CharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) {
         break;
     }
 
-    _rigidBody->setLinearVelocity(finalVelocity);
+    _rigidBody->setLinearVelocity(finalVelocity + _parentVelocity);
 
     // Dynamicaly compute a follow velocity to move this body toward the _followDesiredBodyTransform.
     // Rather then add this velocity to velocity the RigidBody, we explicitly teleport the RigidBody towards its goal.
@@ -383,8 +383,11 @@ void CharacterController::getPositionAndOrientation(glm::vec3& position, glm::qu
 }
 
 void CharacterController::setTargetVelocity(const glm::vec3& velocity) {
-    //_walkVelocity = glmToBullet(_avatarData->getTargetVelocity());
-    _walkVelocity = glmToBullet(velocity);
+    _targetVelocity = glmToBullet(velocity);
+}
+
+void CharacterController::setParentVelocity(const glm::vec3& velocity) {
+    _parentVelocity = glmToBullet(velocity);
 }
 
 void CharacterController::setFollowParameters(const glm::mat4& desiredWorldBodyMatrix, float timeRemaining) {
diff --git a/libraries/physics/src/CharacterController.h b/libraries/physics/src/CharacterController.h
index 86ef350812..5362ca52e4 100644
--- a/libraries/physics/src/CharacterController.h
+++ b/libraries/physics/src/CharacterController.h
@@ -69,6 +69,7 @@ public:
     void getPositionAndOrientation(glm::vec3& position, glm::quat& rotation) const;
 
     void setTargetVelocity(const glm::vec3& velocity);
+    void setParentVelocity(const glm::vec3& parentVelocity);
     void setFollowParameters(const glm::mat4& desiredWorldMatrix, float timeRemaining);
     float getFollowTime() const { return _followTime; }
     glm::vec3 getFollowLinearDisplacement() const;
@@ -105,7 +106,8 @@ protected:
 
 protected:
     btVector3 _currentUp;
-    btVector3 _walkVelocity;
+    btVector3 _targetVelocity;
+    btVector3 _parentVelocity;
     btTransform _followDesiredBodyTransform;
     btScalar _followTimeRemaining;
     btTransform _characterBodyTransform;

From 072172b1a204bc97d62ca13466b734ced5b7f483 Mon Sep 17 00:00:00 2001
From: "Anthony J. Thibault" <tony@highfidelity.io>
Date: Mon, 8 Feb 2016 19:43:23 -0800
Subject: [PATCH 4/7] SpatiallyNestable: now with velocity support!

Moved velocity and angularVelocity into the SpatiallyNestable base class.

Entity velocity and angularVelocity properties are now relative to their parent, similar to the way position and orientation work for entities.

MyAvatar rig animations now use SpatiallyNestable to convert velocity into local frame to drive the animation state machine.
---
 examples/moving-platform.js                |  5 +-
 interface/src/avatar/Avatar.cpp            | 19 ++---
 interface/src/avatar/Avatar.h              |  2 -
 interface/src/avatar/MyAvatar.cpp          | 11 +--
 interface/src/avatar/SkeletonModel.cpp     | 22 +----
 libraries/avatars/src/AvatarData.cpp       |  1 -
 libraries/avatars/src/AvatarData.h         |  6 +-
 libraries/entities/src/EntityItem.cpp      | 40 +++++----
 libraries/entities/src/EntityItem.h        | 10 +--
 libraries/shared/src/SpatiallyNestable.cpp | 99 ++++++++++++++++++++++
 libraries/shared/src/SpatiallyNestable.h   | 20 +++++
 11 files changed, 162 insertions(+), 73 deletions(-)

diff --git a/examples/moving-platform.js b/examples/moving-platform.js
index 94e45bb571..a3022a3904 100644
--- a/examples/moving-platform.js
+++ b/examples/moving-platform.js
@@ -16,8 +16,8 @@ function init() {
         isDynamic: false
     });
     if (platform) {
-        MyAvatar.position = { x: 0, y: 3, z: 0 };
         if (MyAvatar.getParentID() != platform) {
+            MyAvatar.position = { x: 0, y: 3, z: 0 };
             MyAvatar.setParentID(platform);
         }
     }
@@ -34,8 +34,7 @@ function shutdown() {
     }
 }
 
-
-init();
+Script.setTimeout(init, 3000);
 
 Script.update.connect(update);
 Script.scriptEnding.connect(shutdown);
diff --git a/interface/src/avatar/Avatar.cpp b/interface/src/avatar/Avatar.cpp
index b67d176852..2eab67f192 100644
--- a/interface/src/avatar/Avatar.cpp
+++ b/interface/src/avatar/Avatar.cpp
@@ -86,9 +86,7 @@ Avatar::Avatar(RigPointer rig) :
     _positionDeltaAccumulator(0.0f),
     _lastVelocity(0.0f),
     _acceleration(0.0f),
-    _angularVelocity(0.0f),
     _lastAngularVelocity(0.0f),
-    _angularAcceleration(0.0f),
     _lastOrientation(),
     _leanScale(0.5f),
     _worldUpDirection(DEFAULT_UP_DIRECTION),
@@ -235,9 +233,6 @@ void Avatar::simulate(float deltaTime) {
         _displayNameAlpha = abs(_displayNameAlpha - _displayNameTargetAlpha) < 0.01f ? _displayNameTargetAlpha : _displayNameAlpha;
     }
 
-    // NOTE: we shouldn't extrapolate an Avatar instance forward in time...
-    // until velocity is included in AvatarData update message.
-    //_position += _velocity * deltaTime;
     measureMotionDerivatives(deltaTime);
 
     simulateAttachments(deltaTime);
@@ -255,7 +250,7 @@ bool Avatar::isLookingAtMe(AvatarSharedPointer avatar) const {
 void Avatar::slamPosition(const glm::vec3& newPosition) {
     setPosition(newPosition);
     _positionDeltaAccumulator = glm::vec3(0.0f);
-    _velocity = glm::vec3(0.0f);
+    setVelocity(glm::vec3(0.0f));
     _lastVelocity = glm::vec3(0.0f);
 }
 
@@ -269,15 +264,17 @@ void Avatar::measureMotionDerivatives(float deltaTime) {
     float invDeltaTime = 1.0f / deltaTime;
     // Floating point error prevents us from computing velocity in a naive way
     // (e.g. vel = (pos - oldPos) / dt) so instead we use _positionOffsetAccumulator.
-    _velocity = _positionDeltaAccumulator * invDeltaTime;
+    glm::vec3 velocity = _positionDeltaAccumulator * invDeltaTime;
     _positionDeltaAccumulator = glm::vec3(0.0f);
-    _acceleration = (_velocity - _lastVelocity) * invDeltaTime;
-    _lastVelocity = _velocity;
+    _acceleration = (velocity - _lastVelocity) * invDeltaTime;
+    _lastVelocity = velocity;
+    setVelocity(velocity);
+
     // angular
     glm::quat orientation = getOrientation();
     glm::quat delta = glm::inverse(_lastOrientation) * orientation;
-    _angularVelocity = safeEulerAngles(delta) * invDeltaTime;
-    _angularAcceleration = (_angularVelocity - _lastAngularVelocity) * invDeltaTime;
+    glm::vec3 angularVelocity = glm::axis(delta) * glm::angle(delta) * invDeltaTime;
+    setAngularVelocity(angularVelocity);
     _lastOrientation = getOrientation();
 }
 
diff --git a/interface/src/avatar/Avatar.h b/interface/src/avatar/Avatar.h
index b23b64b384..0f84f35a25 100644
--- a/interface/src/avatar/Avatar.h
+++ b/interface/src/avatar/Avatar.h
@@ -135,8 +135,6 @@ public:
     Q_INVOKABLE glm::vec3 getNeckPosition() const;
 
     Q_INVOKABLE glm::vec3 getAcceleration() const { return _acceleration; }
-    Q_INVOKABLE glm::vec3 getAngularVelocity() const { return _angularVelocity; }
-    Q_INVOKABLE glm::vec3 getAngularAcceleration() const { return _angularAcceleration; }
 
     Q_INVOKABLE bool getShouldRender() const { return !_shouldSkipRender; }
 
diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index 1436c192b5..bce08d6e19 100644
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -1079,17 +1079,8 @@ void MyAvatar::rebuildCollisionShape() {
 void MyAvatar::prepareForPhysicsSimulation() {
     relayDriveKeysToCharacterController();
 
-    glm::vec3 parentVelocity;
     bool success;
-    auto parentPtr = getParentPointer(success);
-    if (success && parentPtr && parentPtr->getNestableType() == NestableType::Entity) {
-        auto entityPtr = std::dynamic_pointer_cast<EntityItem>(parentPtr);
-        if (entityPtr) {
-            parentVelocity = entityPtr->getVelocity();
-        }
-    }
-
-    _characterController.setParentVelocity(parentVelocity);
+    _characterController.setParentVelocity(getParentVelocity(success));
     _characterController.setTargetVelocity(getTargetVelocity());
     _characterController.setPositionAndOrientation(getPosition(), getOrientation());
     if (qApp->isHMDMode()) {
diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp
index a942830f68..c77c4fdd78 100644
--- a/interface/src/avatar/SkeletonModel.cpp
+++ b/interface/src/avatar/SkeletonModel.cpp
@@ -149,25 +149,9 @@ void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) {
 
         Rig::CharacterControllerState ccState = convertCharacterControllerState(myAvatar->getCharacterController()->getState());
 
-        auto velocity = myAvatar->getVelocity();
-        auto position = myAvatar->getPosition();
-        auto orientation = myAvatar->getOrientation();
-
-        // check to see if we are attached to an Entity.
-        bool success;
-        auto parentPtr = myAvatar->getParentPointer(success);
-        if (success && parentPtr && parentPtr->getNestableType() == NestableType::Entity) {
-            auto entityPtr = std::dynamic_pointer_cast<EntityItem>(parentPtr);
-            if (entityPtr) {
-                // TODO: Do we need account for angularVelocity of entity?
-
-                auto invParentRot = glm::inverse(entityPtr->getOrientation());
-                velocity = invParentRot * (myAvatar->getVelocity() - entityPtr->getVelocity());
-                position = myAvatar->getLocalPosition();
-                orientation = myAvatar->getLocalOrientation();
-            }
-        }
-
+        auto velocity = myAvatar->getLocalVelocity();
+        auto position = myAvatar->getLocalPosition();
+        auto orientation = myAvatar->getLocalOrientation();
         _rig->computeMotionAnimationState(deltaTime, position, velocity, orientation, ccState);
 
         // evaluate AnimGraph animation and update jointStates.
diff --git a/libraries/avatars/src/AvatarData.cpp b/libraries/avatars/src/AvatarData.cpp
index 3933d705fc..d14df7b05a 100644
--- a/libraries/avatars/src/AvatarData.cpp
+++ b/libraries/avatars/src/AvatarData.cpp
@@ -64,7 +64,6 @@ AvatarData::AvatarData() :
     _billboard(),
     _errorLogExpiry(0),
     _owningAvatarMixer(),
-    _velocity(0.0f),
     _targetVelocity(0.0f),
     _localAABox(DEFAULT_LOCAL_AABOX_CORNER, DEFAULT_LOCAL_AABOX_SCALE)
 {
diff --git a/libraries/avatars/src/AvatarData.h b/libraries/avatars/src/AvatarData.h
index eac1917533..cf6cf80162 100644
--- a/libraries/avatars/src/AvatarData.h
+++ b/libraries/avatars/src/AvatarData.h
@@ -151,6 +151,9 @@ class AvatarData : public QObject, public SpatiallyNestable {
     Q_PROPERTY(float headYaw READ getHeadYaw WRITE setHeadYaw)
     Q_PROPERTY(float headRoll READ getHeadRoll WRITE setHeadRoll)
 
+    Q_PROPERTY(glm::vec3 velocity READ getVelocity WRITE setVelocity)
+    Q_PROPERTY(glm::vec3 angularVelocity READ getAngularVelocity WRITE setAngularVelocity)
+
     Q_PROPERTY(float audioLoudness READ getAudioLoudness WRITE setAudioLoudness)
     Q_PROPERTY(float audioAverageLoudness READ getAudioAverageLoudness WRITE setAudioAverageLoudness)
 
@@ -334,8 +337,6 @@ public:
     int getAverageBytesReceivedPerSecond() const;
     int getReceiveRate() const;
 
-    void setVelocity(const glm::vec3 velocity) { _velocity = velocity; }
-    Q_INVOKABLE glm::vec3 getVelocity() const { return _velocity; }
     const glm::vec3& getTargetVelocity() const { return _targetVelocity; }
 
     bool shouldDie() const { return _owningAvatarMixer.isNull() || getUsecsSinceLastUpdate() > AVATAR_SILENCE_THRESHOLD_USECS; }
@@ -406,7 +407,6 @@ protected:
     /// Loads the joint indices, names from the FST file (if any)
     virtual void updateJointMappings();
 
-    glm::vec3 _velocity;
     glm::vec3 _targetVelocity;
 
     AABox _localAABox;
diff --git a/libraries/entities/src/EntityItem.cpp b/libraries/entities/src/EntityItem.cpp
index a4a646f532..c3859d32de 100644
--- a/libraries/entities/src/EntityItem.cpp
+++ b/libraries/entities/src/EntityItem.cpp
@@ -49,7 +49,6 @@ EntityItem::EntityItem(const EntityItemID& entityItemID) :
     _localRenderAlpha(ENTITY_ITEM_DEFAULT_LOCAL_RENDER_ALPHA),
     _density(ENTITY_ITEM_DEFAULT_DENSITY),
     _volumeMultiplier(1.0f),
-    _velocity(ENTITY_ITEM_DEFAULT_VELOCITY),
     _gravity(ENTITY_ITEM_DEFAULT_GRAVITY),
     _acceleration(ENTITY_ITEM_DEFAULT_ACCELERATION),
     _damping(ENTITY_ITEM_DEFAULT_DAMPING),
@@ -60,7 +59,6 @@ EntityItem::EntityItem(const EntityItemID& entityItemID) :
     _scriptTimestamp(ENTITY_ITEM_DEFAULT_SCRIPT_TIMESTAMP),
     _collisionSoundURL(ENTITY_ITEM_DEFAULT_COLLISION_SOUND_URL),
     _registrationPoint(ENTITY_ITEM_DEFAULT_REGISTRATION_POINT),
-    _angularVelocity(ENTITY_ITEM_DEFAULT_ANGULAR_VELOCITY),
     _angularDamping(ENTITY_ITEM_DEFAULT_ANGULAR_DAMPING),
     _visible(ENTITY_ITEM_DEFAULT_VISIBLE),
     _collisionless(ENTITY_ITEM_DEFAULT_COLLISIONLESS),
@@ -78,6 +76,8 @@ EntityItem::EntityItem(const EntityItemID& entityItemID) :
     _physicsInfo(nullptr),
     _simulated(false)
 {
+    setVelocity(ENTITY_ITEM_DEFAULT_VELOCITY);
+    setAngularVelocity(ENTITY_ITEM_DEFAULT_ANGULAR_VELOCITY);
     // explicitly set transform parts to set dirty flags used by batch rendering
     setScale(ENTITY_ITEM_DEFAULT_DIMENSIONS);
     quint64 now = usecTimestampNow();
@@ -886,41 +886,45 @@ void EntityItem::simulateKinematicMotion(float timeElapsed, bool setFlags) {
     }
 
     if (hasAngularVelocity()) {
+        glm::vec3 angularVelocity = getAngularVelocity();
+
         // angular damping
         if (_angularDamping > 0.0f) {
-            _angularVelocity *= powf(1.0f - _angularDamping, timeElapsed);
+            angularVelocity *= powf(1.0f - _angularDamping, timeElapsed);
             #ifdef WANT_DEBUG
                 qCDebug(entities) << "    angularDamping :" << _angularDamping;
-                qCDebug(entities) << "    newAngularVelocity:" << _angularVelocity;
+                qCDebug(entities) << "    newAngularVelocity:" << angularVelocity;
             #endif
         }
 
-        float angularSpeed = glm::length(_angularVelocity);
+        float angularSpeed = glm::length(angularVelocity);
 
         const float EPSILON_ANGULAR_VELOCITY_LENGTH = 0.0017453f; // 0.0017453 rad/sec = 0.1f degrees/sec
         if (angularSpeed < EPSILON_ANGULAR_VELOCITY_LENGTH) {
             if (setFlags && angularSpeed > 0.0f) {
                 _dirtyFlags |= Simulation::DIRTY_MOTION_TYPE;
             }
-            _angularVelocity = ENTITY_ITEM_ZERO_VEC3;
+            angularVelocity = ENTITY_ITEM_ZERO_VEC3;
         } else {
             // for improved agreement with the way Bullet integrates rotations we use an approximation
             // and break the integration into bullet-sized substeps
             glm::quat rotation = getRotation();
             float dt = timeElapsed;
             while (dt > PHYSICS_ENGINE_FIXED_SUBSTEP) {
-                glm::quat  dQ = computeBulletRotationStep(_angularVelocity, PHYSICS_ENGINE_FIXED_SUBSTEP);
+                glm::quat  dQ = computeBulletRotationStep(angularVelocity, PHYSICS_ENGINE_FIXED_SUBSTEP);
                 rotation = glm::normalize(dQ * rotation);
                 dt -= PHYSICS_ENGINE_FIXED_SUBSTEP;
             }
             // NOTE: this final partial substep can drift away from a real Bullet simulation however
             // it only becomes significant for rapidly rotating objects
             // (e.g. around PI/4 radians per substep, or 7.5 rotations/sec at 60 substeps/sec).
-            glm::quat  dQ = computeBulletRotationStep(_angularVelocity, dt);
+            glm::quat  dQ = computeBulletRotationStep(angularVelocity, dt);
             rotation = glm::normalize(dQ * rotation);
 
             setRotation(rotation);
         }
+
+        setAngularVelocity(angularVelocity);
     }
 
     if (hasVelocity()) {
@@ -1077,9 +1081,9 @@ EntityItemProperties EntityItem::getProperties(EntityPropertyFlags desiredProper
 void EntityItem::getAllTerseUpdateProperties(EntityItemProperties& properties) const {
     // a TerseUpdate includes the transform and its derivatives
     properties._position = getLocalPosition();
-    properties._velocity = _velocity;
+    properties._velocity = getLocalVelocity();
     properties._rotation = getLocalOrientation();
-    properties._angularVelocity = _angularVelocity;
+    properties._angularVelocity = getLocalAngularVelocity();
     properties._acceleration = _acceleration;
 
     properties._positionChanged = true;
@@ -1406,13 +1410,15 @@ void EntityItem::updateVelocity(const glm::vec3& value) {
     if (shouldSuppressLocationEdits()) {
         return;
     }
-    if (_velocity != value) {
+    glm::vec3 velocity = getLocalVelocity();
+    if (velocity != value) {
         const float MIN_LINEAR_SPEED = 0.001f;
         if (glm::length(value) < MIN_LINEAR_SPEED) {
-            _velocity = ENTITY_ITEM_ZERO_VEC3;
+            velocity = ENTITY_ITEM_ZERO_VEC3;
         } else {
-            _velocity = value;
+            velocity = value;
         }
+        setLocalVelocity(velocity);
         _dirtyFlags |= Simulation::DIRTY_LINEAR_VELOCITY;
     }
 }
@@ -1436,13 +1442,15 @@ void EntityItem::updateAngularVelocity(const glm::vec3& value) {
     if (shouldSuppressLocationEdits()) {
         return;
     }
-    if (_angularVelocity != value) {
+    glm::vec3 angularVelocity = getLocalAngularVelocity();
+    if (angularVelocity != value) {
         const float MIN_ANGULAR_SPEED = 0.0002f;
         if (glm::length(value) < MIN_ANGULAR_SPEED) {
-            _angularVelocity = ENTITY_ITEM_ZERO_VEC3;
+            angularVelocity = ENTITY_ITEM_ZERO_VEC3;
         } else {
-            _angularVelocity = value;
+            angularVelocity = value;
         }
+        setLocalAngularVelocity(angularVelocity);
         _dirtyFlags |= Simulation::DIRTY_ANGULAR_VELOCITY;
     }
 }
diff --git a/libraries/entities/src/EntityItem.h b/libraries/entities/src/EntityItem.h
index 8271aedb15..c8b54bca87 100644
--- a/libraries/entities/src/EntityItem.h
+++ b/libraries/entities/src/EntityItem.h
@@ -193,9 +193,7 @@ public:
 
     float getDensity() const { return _density; }
 
-    const glm::vec3& getVelocity() const { return _velocity; } /// get velocity in meters
-    void setVelocity(const glm::vec3& value) { _velocity = value; } /// velocity in meters
-    bool hasVelocity() const { return _velocity != ENTITY_ITEM_ZERO_VEC3; }
+    bool hasVelocity() const { return getVelocity() != ENTITY_ITEM_ZERO_VEC3; }
 
     const glm::vec3& getGravity() const { return _gravity; } /// get gravity in meters
     void setGravity(const glm::vec3& value) { _gravity = value; } /// gravity in meters
@@ -255,9 +253,7 @@ public:
     void setRegistrationPoint(const glm::vec3& value)
             { _registrationPoint = glm::clamp(value, 0.0f, 1.0f); requiresRecalcBoxes(); }
 
-    const glm::vec3& getAngularVelocity() const { return _angularVelocity; }
-    void setAngularVelocity(const glm::vec3& value) { _angularVelocity = value; }
-    bool hasAngularVelocity() const { return _angularVelocity != ENTITY_ITEM_ZERO_VEC3; }
+    bool hasAngularVelocity() const { return getAngularVelocity() != ENTITY_ITEM_ZERO_VEC3; }
 
     float getAngularDamping() const { return _angularDamping; }
     void setAngularDamping(float value) { _angularDamping = value; }
@@ -435,7 +431,6 @@ protected:
     // rather than in all of the derived classes.  If we ever collapse these classes to one we could do it a
     // different way.
     float _volumeMultiplier = 1.0f;
-    glm::vec3 _velocity;
     glm::vec3 _gravity;
     glm::vec3 _acceleration;
     float _damping;
@@ -446,7 +441,6 @@ protected:
     quint64 _scriptTimestamp;
     QString _collisionSoundURL;
     glm::vec3 _registrationPoint;
-    glm::vec3 _angularVelocity;
     float _angularDamping;
     bool _visible;
     bool _collisionless;
diff --git a/libraries/shared/src/SpatiallyNestable.cpp b/libraries/shared/src/SpatiallyNestable.cpp
index e5719c17ff..3db714964f 100644
--- a/libraries/shared/src/SpatiallyNestable.cpp
+++ b/libraries/shared/src/SpatiallyNestable.cpp
@@ -391,6 +391,77 @@ void SpatiallyNestable::setOrientation(const glm::quat& orientation) {
     #endif
 }
 
+glm::vec3 SpatiallyNestable::getVelocity(bool& success) const {
+    glm::vec3 parentVelocity = getParentVelocity(success);
+    Transform parentTransform = getParentTransform(success);
+    glm::vec3 result;
+    _transformLock.withReadLock([&] {
+        // TODO: take parent angularVelocity into account.
+        result = parentVelocity + parentTransform.getRotation() * _velocity;
+    });
+    return result;
+}
+
+glm::vec3 SpatiallyNestable::getVelocity() const {
+    bool success;
+    return getVelocity(success);
+}
+
+void SpatiallyNestable::setVelocity(const glm::vec3& velocity, bool& success) {
+    glm::vec3 parentVelocity = getParentVelocity(success);
+    Transform parentTransform = getParentTransform(success);
+    _transformLock.withWriteLock([&] {
+        // TODO: take parent angularVelocity into account.
+        _velocity = glm::inverse(parentTransform.getRotation()) * velocity - parentVelocity;
+    });
+}
+
+void SpatiallyNestable::setVelocity(const glm::vec3& velocity) {
+    bool success;
+    setVelocity(velocity, success);
+}
+
+glm::vec3 SpatiallyNestable::getParentVelocity(bool& success) const {
+    glm::vec3 result;
+    SpatiallyNestablePointer parent = getParentPointer(success);
+    if (success && parent) {
+        result = parent->getVelocity(success);
+    }
+    return result;
+}
+
+glm::vec3 SpatiallyNestable::getAngularVelocity(bool& success) const {
+    glm::vec3 parentAngularVelocity = getParentAngularVelocity(success);
+    Transform parentTransform = getParentTransform(success);
+    glm::vec3 result;
+    _transformLock.withReadLock([&] {
+        result = parentAngularVelocity + parentTransform.getRotation() * _angularVelocity;
+    });
+    return result;
+}
+
+glm::vec3 SpatiallyNestable::getAngularVelocity() const {
+    bool success;
+    return getAngularVelocity(success);
+}
+
+void SpatiallyNestable::setAngularVelocity(const glm::vec3& angularVelocity, bool& success) {
+    glm::vec3 parentAngularVelocity = getParentAngularVelocity(success);
+    Transform parentTransform = getParentTransform(success);
+    _transformLock.withWriteLock([&] {
+        _angularVelocity = glm::inverse(parentTransform.getRotation()) * angularVelocity - parentAngularVelocity;
+    });
+}
+
+void SpatiallyNestable::setAngularVelocity(const glm::vec3& angularVelocity) {
+    bool success;
+    setAngularVelocity(angularVelocity, success);
+}
+
+glm::vec3 SpatiallyNestable::getParentAngularVelocity(bool& success) const {
+    return glm::vec3();
+}
+
 const Transform SpatiallyNestable::getTransform(bool& success) const {
     // return a world-space transform for this object's location
     Transform parentTransform = getParentTransform(success);
@@ -519,6 +590,34 @@ void SpatiallyNestable::setLocalOrientation(const glm::quat& orientation) {
     locationChanged();
 }
 
+glm::vec3 SpatiallyNestable::getLocalVelocity() const {
+    glm::vec3 result(glm::vec3::_null);
+    _transformLock.withReadLock([&] {
+        result = _velocity;
+    });
+    return result;
+}
+
+void SpatiallyNestable::setLocalVelocity(const glm::vec3& velocity) {
+    _transformLock.withWriteLock([&] {
+        _velocity = velocity;
+    });
+}
+
+glm::vec3 SpatiallyNestable::getLocalAngularVelocity() const {
+    glm::vec3 result(glm::vec3::_null);
+    _transformLock.withReadLock([&] {
+        result = _angularVelocity;
+    });
+    return result;
+}
+
+void SpatiallyNestable::setLocalAngularVelocity(const glm::vec3& angularVelocity) {
+    _transformLock.withWriteLock([&] {
+        _angularVelocity = angularVelocity;
+    });
+}
+
 glm::vec3 SpatiallyNestable::getLocalScale() const {
     // TODO: scale
     glm::vec3 result;
diff --git a/libraries/shared/src/SpatiallyNestable.h b/libraries/shared/src/SpatiallyNestable.h
index 8153316ed9..ad2a9e2dd2 100644
--- a/libraries/shared/src/SpatiallyNestable.h
+++ b/libraries/shared/src/SpatiallyNestable.h
@@ -68,6 +68,18 @@ public:
     virtual void setOrientation(const glm::quat& orientation, bool& success);
     virtual void setOrientation(const glm::quat& orientation);
 
+    virtual glm::vec3 getVelocity(bool& success) const;
+    virtual glm::vec3 getVelocity() const;
+    virtual void setVelocity(const glm::vec3& velocity, bool& success);
+    virtual void setVelocity(const glm::vec3& velocity);
+    virtual glm::vec3 getParentVelocity(bool& success) const;
+
+    virtual glm::vec3 getAngularVelocity(bool& success) const;
+    virtual glm::vec3 getAngularVelocity() const;
+    virtual void setAngularVelocity(const glm::vec3& angularVelocity, bool& success);
+    virtual void setAngularVelocity(const glm::vec3& angularVelocity);
+    virtual glm::vec3 getParentAngularVelocity(bool& success) const;
+
     virtual AACube getMaximumAACube(bool& success) const;
     virtual bool computePuffedQueryAACube();
 
@@ -94,6 +106,12 @@ public:
     virtual glm::quat getLocalOrientation() const;
     virtual void setLocalOrientation(const glm::quat& orientation);
 
+    virtual glm::vec3 getLocalVelocity() const;
+    virtual void setLocalVelocity(const glm::vec3& velocity);
+
+    virtual glm::vec3 getLocalAngularVelocity() const;
+    virtual void setLocalAngularVelocity(const glm::vec3& angularVelocity);
+
     virtual glm::vec3 getLocalScale() const;
     virtual void setLocalScale(const glm::vec3& scale);
 
@@ -148,6 +166,8 @@ private:
     mutable ReadWriteLockable _transformLock;
     mutable ReadWriteLockable _idLock;
     Transform _transform; // this is to be combined with parent's world-transform to produce this' world-transform.
+    glm::vec3 _velocity;
+    glm::vec3 _angularVelocity;
     mutable bool _parentKnowsMe { false };
     bool _isDead { false };
 };

From 9a0e206843801d99fda7e0943cab808fd5e7a360 Mon Sep 17 00:00:00 2001
From: "Anthony J. Thibault" <tony@highfidelity.io>
Date: Mon, 8 Feb 2016 20:04:45 -0800
Subject: [PATCH 5/7] SpatiallyNestable: made getParentPointer protected again.

---
 libraries/shared/src/SpatiallyNestable.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/libraries/shared/src/SpatiallyNestable.h b/libraries/shared/src/SpatiallyNestable.h
index ad2a9e2dd2..a4f4691c40 100644
--- a/libraries/shared/src/SpatiallyNestable.h
+++ b/libraries/shared/src/SpatiallyNestable.h
@@ -137,13 +137,13 @@ public:
     bool isDead() const { return _isDead; }
 
     bool isParentIDValid() const { bool success = false; getParentPointer(success); return success; }
-    SpatiallyNestablePointer getParentPointer(bool& success) const;
 
 protected:
     const NestableType _nestableType; // EntityItem or an AvatarData
     QUuid _id;
     QUuid _parentID; // what is this thing's transform relative to?
     quint16 _parentJointIndex { 0 }; // which joint of the parent is this relative to?
+    SpatiallyNestablePointer getParentPointer(bool& success) const;
 
     mutable SpatiallyNestableWeakPointer _parent;
 

From 6a82594a4f50463fe9254dc6060fbb9aa4cf8562 Mon Sep 17 00:00:00 2001
From: "Anthony J. Thibault" <tony@highfidelity.io>
Date: Mon, 8 Feb 2016 20:09:32 -0800
Subject: [PATCH 6/7] SpatiallyNestable: more locks, less contention

---
 libraries/shared/src/SpatiallyNestable.cpp | 16 ++++++++--------
 libraries/shared/src/SpatiallyNestable.h   |  2 ++
 2 files changed, 10 insertions(+), 8 deletions(-)

diff --git a/libraries/shared/src/SpatiallyNestable.cpp b/libraries/shared/src/SpatiallyNestable.cpp
index 3db714964f..48238977d1 100644
--- a/libraries/shared/src/SpatiallyNestable.cpp
+++ b/libraries/shared/src/SpatiallyNestable.cpp
@@ -395,7 +395,7 @@ glm::vec3 SpatiallyNestable::getVelocity(bool& success) const {
     glm::vec3 parentVelocity = getParentVelocity(success);
     Transform parentTransform = getParentTransform(success);
     glm::vec3 result;
-    _transformLock.withReadLock([&] {
+    _velocityLock.withReadLock([&] {
         // TODO: take parent angularVelocity into account.
         result = parentVelocity + parentTransform.getRotation() * _velocity;
     });
@@ -410,7 +410,7 @@ glm::vec3 SpatiallyNestable::getVelocity() const {
 void SpatiallyNestable::setVelocity(const glm::vec3& velocity, bool& success) {
     glm::vec3 parentVelocity = getParentVelocity(success);
     Transform parentTransform = getParentTransform(success);
-    _transformLock.withWriteLock([&] {
+    _velocityLock.withWriteLock([&] {
         // TODO: take parent angularVelocity into account.
         _velocity = glm::inverse(parentTransform.getRotation()) * velocity - parentVelocity;
     });
@@ -434,7 +434,7 @@ glm::vec3 SpatiallyNestable::getAngularVelocity(bool& success) const {
     glm::vec3 parentAngularVelocity = getParentAngularVelocity(success);
     Transform parentTransform = getParentTransform(success);
     glm::vec3 result;
-    _transformLock.withReadLock([&] {
+    _angularVelocityLock.withReadLock([&] {
         result = parentAngularVelocity + parentTransform.getRotation() * _angularVelocity;
     });
     return result;
@@ -448,7 +448,7 @@ glm::vec3 SpatiallyNestable::getAngularVelocity() const {
 void SpatiallyNestable::setAngularVelocity(const glm::vec3& angularVelocity, bool& success) {
     glm::vec3 parentAngularVelocity = getParentAngularVelocity(success);
     Transform parentTransform = getParentTransform(success);
-    _transformLock.withWriteLock([&] {
+    _angularVelocityLock.withWriteLock([&] {
         _angularVelocity = glm::inverse(parentTransform.getRotation()) * angularVelocity - parentAngularVelocity;
     });
 }
@@ -592,28 +592,28 @@ void SpatiallyNestable::setLocalOrientation(const glm::quat& orientation) {
 
 glm::vec3 SpatiallyNestable::getLocalVelocity() const {
     glm::vec3 result(glm::vec3::_null);
-    _transformLock.withReadLock([&] {
+    _velocityLock.withReadLock([&] {
         result = _velocity;
     });
     return result;
 }
 
 void SpatiallyNestable::setLocalVelocity(const glm::vec3& velocity) {
-    _transformLock.withWriteLock([&] {
+    _velocityLock.withWriteLock([&] {
         _velocity = velocity;
     });
 }
 
 glm::vec3 SpatiallyNestable::getLocalAngularVelocity() const {
     glm::vec3 result(glm::vec3::_null);
-    _transformLock.withReadLock([&] {
+    _angularVelocityLock.withReadLock([&] {
         result = _angularVelocity;
     });
     return result;
 }
 
 void SpatiallyNestable::setLocalAngularVelocity(const glm::vec3& angularVelocity) {
-    _transformLock.withWriteLock([&] {
+    _angularVelocityLock.withWriteLock([&] {
         _angularVelocity = angularVelocity;
     });
 }
diff --git a/libraries/shared/src/SpatiallyNestable.h b/libraries/shared/src/SpatiallyNestable.h
index a4f4691c40..0c43aff20d 100644
--- a/libraries/shared/src/SpatiallyNestable.h
+++ b/libraries/shared/src/SpatiallyNestable.h
@@ -165,6 +165,8 @@ protected:
 private:
     mutable ReadWriteLockable _transformLock;
     mutable ReadWriteLockable _idLock;
+    mutable ReadWriteLockable _velocityLock;
+    mutable ReadWriteLockable _angularVelocityLock;
     Transform _transform; // this is to be combined with parent's world-transform to produce this' world-transform.
     glm::vec3 _velocity;
     glm::vec3 _angularVelocity;

From 2b10fea00620341c5bb39731e8720309b3072995 Mon Sep 17 00:00:00 2001
From: "Anthony J. Thibault" <ajt@highfidelity.io>
Date: Tue, 9 Feb 2016 14:03:39 -0800
Subject: [PATCH 7/7] SpatiallyNestable: warn on failure.

---
 interface/src/avatar/MyAvatar.cpp          |  8 ++++++-
 libraries/shared/src/SpatiallyNestable.cpp | 25 +++++++++++++++++++---
 2 files changed, 29 insertions(+), 4 deletions(-)

diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index bce08d6e19..4fdca3840e 100644
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -1080,7 +1080,13 @@ void MyAvatar::prepareForPhysicsSimulation() {
     relayDriveKeysToCharacterController();
 
     bool success;
-    _characterController.setParentVelocity(getParentVelocity(success));
+    glm::vec3 parentVelocity = getParentVelocity(success);
+    if (!success) {
+        qDebug() << "Warning: getParentVelocity failed" << getID();
+        parentVelocity = glm::vec3();
+    }
+    _characterController.setParentVelocity(parentVelocity);
+
     _characterController.setTargetVelocity(getTargetVelocity());
     _characterController.setPositionAndOrientation(getPosition(), getOrientation());
     if (qApp->isHMDMode()) {
diff --git a/libraries/shared/src/SpatiallyNestable.cpp b/libraries/shared/src/SpatiallyNestable.cpp
index 48238977d1..d3048194b4 100644
--- a/libraries/shared/src/SpatiallyNestable.cpp
+++ b/libraries/shared/src/SpatiallyNestable.cpp
@@ -404,7 +404,11 @@ glm::vec3 SpatiallyNestable::getVelocity(bool& success) const {
 
 glm::vec3 SpatiallyNestable::getVelocity() const {
     bool success;
-    return getVelocity(success);
+    glm::vec3 result = getVelocity(success);
+    if (!success) {
+        qDebug() << "Warning -- setVelocity failed" << getID();
+    }
+    return result;
 }
 
 void SpatiallyNestable::setVelocity(const glm::vec3& velocity, bool& success) {
@@ -419,6 +423,9 @@ void SpatiallyNestable::setVelocity(const glm::vec3& velocity, bool& success) {
 void SpatiallyNestable::setVelocity(const glm::vec3& velocity) {
     bool success;
     setVelocity(velocity, success);
+    if (!success) {
+        qDebug() << "Warning -- setVelocity failed" << getID();
+    }
 }
 
 glm::vec3 SpatiallyNestable::getParentVelocity(bool& success) const {
@@ -442,7 +449,11 @@ glm::vec3 SpatiallyNestable::getAngularVelocity(bool& success) const {
 
 glm::vec3 SpatiallyNestable::getAngularVelocity() const {
     bool success;
-    return getAngularVelocity(success);
+    glm::vec3 result = getAngularVelocity(success);
+    if (!success) {
+        qDebug() << "Warning -- getAngularVelocity failed" << getID();
+    }
+    return result;
 }
 
 void SpatiallyNestable::setAngularVelocity(const glm::vec3& angularVelocity, bool& success) {
@@ -456,10 +467,18 @@ void SpatiallyNestable::setAngularVelocity(const glm::vec3& angularVelocity, boo
 void SpatiallyNestable::setAngularVelocity(const glm::vec3& angularVelocity) {
     bool success;
     setAngularVelocity(angularVelocity, success);
+    if (!success) {
+        qDebug() << "Warning -- setAngularVelocity failed" << getID();
+    }
 }
 
 glm::vec3 SpatiallyNestable::getParentAngularVelocity(bool& success) const {
-    return glm::vec3();
+    glm::vec3 result;
+    SpatiallyNestablePointer parent = getParentPointer(success);
+    if (success && parent) {
+        result = parent->getAngularVelocity(success);
+    }
+    return result;
 }
 
 const Transform SpatiallyNestable::getTransform(bool& success) const {