From 89c848d8c8c4351d656361f8a6c0fbdd2ba02be9 Mon Sep 17 00:00:00 2001
From: Seth Alves <seth@highfidelity.io>
Date: Fri, 2 Oct 2015 17:14:53 -0700
Subject: [PATCH] fix up kinematic hold

---
 examples/controllers/handControllerGrab.js    |  2 +-
 .../src/avatar/AvatarActionKinematicHold.cpp  | 24 +++++++++++++++++--
 .../src/avatar/AvatarActionKinematicHold.h    |  4 ++++
 .../entities/src/EntityActionInterface.cpp    |  2 +-
 4 files changed, 28 insertions(+), 4 deletions(-)

diff --git a/examples/controllers/handControllerGrab.js b/examples/controllers/handControllerGrab.js
index 5705bd4498..a8a95cb4ca 100644
--- a/examples/controllers/handControllerGrab.js
+++ b/examples/controllers/handControllerGrab.js
@@ -444,7 +444,7 @@ function MyController(hand, triggerAction) {
         var offsetPosition = Vec3.multiplyQbyV(Quat.inverse(Quat.multiply(handRotation, offsetRotation)), offset);
 
         this.actionID = NULL_ACTION_ID;
-        this.actionID = Entities.addAction("hold", this.grabbedEntity, {
+        this.actionID = Entities.addAction("kinematic-hold", this.grabbedEntity, {
             hand: this.hand === RIGHT_HAND ? "right" : "left",
             timeScale: NEAR_GRABBING_ACTION_TIMEFRAME,
             relativePosition: offsetPosition,
diff --git a/interface/src/avatar/AvatarActionKinematicHold.cpp b/interface/src/avatar/AvatarActionKinematicHold.cpp
index 14b80efd64..e890d8cbd8 100644
--- a/interface/src/avatar/AvatarActionKinematicHold.cpp
+++ b/interface/src/avatar/AvatarActionKinematicHold.cpp
@@ -22,7 +22,9 @@ AvatarActionKinematicHold::AvatarActionKinematicHold(const QUuid& id, EntityItem
     _relativePosition(glm::vec3(0.0f)),
     _relativeRotation(glm::quat()),
     _hand("right"),
-    _mine(false)
+    _mine(false),
+    _previousPositionalTarget(Vectors::ZERO),
+    _previousRotationalTarget(Quaternions::IDENTITY)
 {
     _type = ACTION_TYPE_HOLD;
     #if WANT_DEBUG
@@ -44,6 +46,10 @@ void AvatarActionKinematicHold::updateActionWorker(float deltaTimeStep) {
         return;
     }
 
+    if (deltaTimeStep <= 0.0f) {
+        return;
+    }
+
     glm::quat rotation;
     glm::vec3 position;
     glm::vec3 offset;
@@ -81,10 +87,24 @@ void AvatarActionKinematicHold::updateActionWorker(float deltaTimeStep) {
                             return;
                         }
 
-                        btTransform worldTrans;
+                        btTransform worldTrans = rigidBody->getWorldTransform();
                         worldTrans.setOrigin(glmToBullet(_positionalTarget));
                         worldTrans.setRotation(glmToBullet(_rotationalTarget));
                         rigidBody->setWorldTransform(worldTrans);
+
+                        if (_previousSet) {
+                            glm::vec3 positionalVelocity = (_positionalTarget - _previousPositionalTarget) / deltaTimeStep;
+                            rigidBody->setLinearVelocity(glmToBullet(positionalVelocity));
+
+                            glm::quat rotationalDelta = glm::inverse(_previousRotationalTarget) * _rotationalTarget;
+                            glm::vec3 rotationalDeltaAxis = glm::axis(rotationalDelta);
+                            float rotationalDeltaAngle = glm::angle(rotationalDelta);
+                            glm::vec3 rotationalVelocity = glm::normalize(rotationalDeltaAxis) * rotationalDeltaAngle / deltaTimeStep;
+                            rigidBody->setAngularVelocity(glmToBullet(rotationalVelocity));
+                        }
+                        _previousPositionalTarget = _positionalTarget;
+                        _previousRotationalTarget = _rotationalTarget;
+                        _previousSet = true;
                     }
                 }
             }
diff --git a/interface/src/avatar/AvatarActionKinematicHold.h b/interface/src/avatar/AvatarActionKinematicHold.h
index 6bd7dca01a..95e9605c2b 100644
--- a/interface/src/avatar/AvatarActionKinematicHold.h
+++ b/interface/src/avatar/AvatarActionKinematicHold.h
@@ -36,6 +36,10 @@ private:
     glm::quat _relativeRotation;
     QString _hand;
     bool _mine = false;
+
+    bool _previousSet = false;
+    glm::vec3 _previousPositionalTarget;
+    glm::quat _previousRotationalTarget;
 };
 
 #endif // hifi_AvatarActionKinematicHold_h
diff --git a/libraries/entities/src/EntityActionInterface.cpp b/libraries/entities/src/EntityActionInterface.cpp
index d821dcdc2f..25f61fcf25 100644
--- a/libraries/entities/src/EntityActionInterface.cpp
+++ b/libraries/entities/src/EntityActionInterface.cpp
@@ -100,7 +100,7 @@ EntityActionType EntityActionInterface::actionTypeFromString(QString actionTypeS
     if (normalizedActionTypeString == "hold") {
         return ACTION_TYPE_HOLD;
     }
-    if (normalizedActionTypeString == "kinematic-hold") {
+    if (normalizedActionTypeString == "kinematichold") {
         return ACTION_TYPE_KINEMATIC_HOLD;
     }