Fix scale and add rayTest

interface/src/Application.cpp

@@ -159,6 +159,7 @@
 #include "avatar/AvatarManager.h"
 #include "avatar/MyHead.h"
 #include "avatar/AvatarPackager.h"
+#include "avatar/MyCharacterController.h"
 #include "CrashRecoveryHandler.h"
 #include "CrashHandler.h"
 #include "devices/DdeFaceTracker.h"
@@ -2636,6 +2637,13 @@ Application::~Application() {
 
     avatarManager->deleteAllAvatars();
     
+    auto myCharacterController = getMyAvatar()->getCharacterController();
+    myCharacterController->clearDetailedMotionStates();
+
+    myCharacterController->buildPhysicsTransaction(transaction);
+    _physicsEngine->processTransaction(transaction);
+    myCharacterController->handleProcessedPhysicsTransaction(transaction);
+    
     _physicsEngine->setCharacterController(nullptr);
 
     // the _shapeManager should have zero references
@@ -6123,7 +6131,9 @@ void Application::update(float deltaTime) {
                 avatarManager->buildPhysicsTransaction(transaction);
                 _physicsEngine->processTransaction(transaction);
                 avatarManager->handleProcessedPhysicsTransaction(transaction);
-
+                myAvatar->getCharacterController()->buildPhysicsTransaction(transaction);
+                _physicsEngine->processTransaction(transaction);
+                myAvatar->getCharacterController()->handleProcessedPhysicsTransaction(transaction);
                 myAvatar->prepareForPhysicsSimulation();
                 _physicsEngine->forEachDynamic([&](EntityDynamicPointer dynamic) {
                     dynamic->prepareForPhysicsSimulation();

interface/src/avatar/AvatarManager.cpp

@@ -415,7 +415,6 @@ void AvatarManager::buildPhysicsTransaction(PhysicsEngine::Transaction& transact
                 }
                 qCDebug(animation) << "Removing " << detailedMotionStates.size() << " detailed motion states from " << avatar->getSessionUUID();
                 avatar->resetDetailedMotionStates();
-
             } else {
                 if (avatar->_motionState == nullptr) {
                     ShapeInfo shapeInfo;
@@ -549,7 +548,8 @@ void AvatarManager::deleteAllAvatars() {
         avatar->die();
         if (avatar != _myAvatar) {
             auto otherAvatar = std::static_pointer_cast<OtherAvatar>(avatar);
-            assert(!otherAvatar->_motionState && !otherAvatar->_motionState2);
+            assert(!otherAvatar->_motionState);
+            assert(otherAvatar->getDetailedMotionStates().size() == 0);            
         }
     }
 }
@@ -662,83 +662,21 @@ RayToAvatarIntersectionResult AvatarManager::findRayIntersectionVector(const Pic
         return result;
     }
 
-    // It's better to intersect the ray against the avatar's actual mesh, but this is currently difficult to
+    float distance = (float)INT_MAX;  // with FLT_MAX bullet rayTest does not return results 
-    // do, because the transformed mesh data only exists over in GPU-land.  As a compromise, this code
+    BoxFace face = BoxFace::UNKNOWN_FACE;
-    // intersects against the avatars capsule and then against the (T-pose) mesh.  The end effect is that picking
-    // against the avatar is sort-of right, but you likely wont be able to pick against the arms.
-
-    // TODO -- find a way to extract transformed avatar mesh data from the rendering engine.
-
-    std::vector<SortedAvatar> sortedAvatars;
-    auto avatarHashCopy = getHashCopy();
-    for (auto avatarData : avatarHashCopy) {
-        auto avatar = std::static_pointer_cast<Avatar>(avatarData);
-        if ((avatarsToInclude.size() > 0 && !avatarsToInclude.contains(avatar->getID())) ||
-            (avatarsToDiscard.size() > 0 && avatarsToDiscard.contains(avatar->getID()))) {
-            continue;
-        }
-
-        float distance = FLT_MAX;
-#if 0
-        // if we weren't picking against the capsule, we would want to pick against the avatarBounds...
-        SkeletonModelPointer avatarModel = avatar->getSkeletonModel();
-        AABox avatarBounds = avatarModel->getRenderableMeshBound();
-        if (avatarBounds.contains(ray.origin)) {
-            distance = 0.0f;
-        } else {
-            float boundDistance = FLT_MAX;
-            BoxFace face;
-            glm::vec3 surfaceNormal;
-            if (avatarBounds.findRayIntersection(ray.origin, ray.direction, boundDistance, face, surfaceNormal)) {
-                distance = boundDistance;
-            }
-        }
-#else
-        glm::vec3 start;
-        glm::vec3 end;
-        float radius;
-        avatar->getCapsule(start, end, radius);
-        findRayCapsuleIntersection(ray.origin, ray.direction, start, end, radius, distance);
-#endif
-
-        if (distance < FLT_MAX) {
-            sortedAvatars.emplace_back(distance, avatar);
-        }
-    }
-
-    if (sortedAvatars.size() > 1) {
-        static auto comparator = [](const SortedAvatar& left, const SortedAvatar& right) { return left.first < right.first; };
-        std::sort(sortedAvatars.begin(), sortedAvatars.end(), comparator);
-    }
-
-    for (auto it = sortedAvatars.begin(); it != sortedAvatars.end(); ++it) {
-        const SortedAvatar& sortedAvatar = *it;
-        // We can exit once avatarCapsuleDistance > bestDistance
-        if (sortedAvatar.first > result.distance) {
-            break;
-        }
-
-        float distance = FLT_MAX;
-        BoxFace face;
     glm::vec3 surfaceNormal;
     QVariantMap extraInfo;
-        SkeletonModelPointer avatarModel = sortedAvatar.second->getSkeletonModel();
+    MyCharacterController::RayAvatarResult physicsResult = _myAvatar->getCharacterController()->rayTest(glmToBullet(ray.origin), glmToBullet(ray.direction), distance, QVector<uint>());
-        if (avatarModel->findRayIntersectionAgainstSubMeshes(ray.origin, ray.direction, distance, face, surfaceNormal, extraInfo, true)) {
+    if (physicsResult._intersect) {
-            if (distance < result.distance) {
         result.intersects = true;
-                result.avatarID = sortedAvatar.second->getID();
+        result.avatarID = physicsResult._intersectWithAvatar;
-                result.distance = distance;
+        result.distance = physicsResult._distance;
-                result.face = face;
+        result.surfaceNormal = physicsResult._intersectionNormal;
-                result.surfaceNormal = surfaceNormal;
+        result.jointIndex = physicsResult._intersectWithJoint;
+        result.intersection = physicsResult._intersectionPoint;
         result.extraInfo = extraInfo;
+        result.face = face;
     }
-        }
-    }
-
-    if (result.intersects) {
-        result.intersection = ray.origin + ray.direction * result.distance;
-    }
-
     return result;
 }
 
@@ -836,6 +774,42 @@ ParabolaToAvatarIntersectionResult AvatarManager::findParabolaIntersectionVector
     return result;
 }
 
+RayToAvatarIntersectionResult AvatarManager::findSelfRayIntersection(const PickRay& ray,
+    const QScriptValue& jointIndexesToInclude,
+    const QScriptValue& jointIndexesToDiscard) {
+    QVector<uint> jointsToInclude;
+    QVector<uint> jointsToDiscard;
+    qVectorIntFromScriptValue(jointIndexesToInclude, jointsToInclude);
+    qVectorIntFromScriptValue(jointIndexesToDiscard, jointsToDiscard);
+    return findSelfRayIntersectionVector(ray, jointsToInclude, jointsToDiscard);
+}
+
+RayToAvatarIntersectionResult AvatarManager::findSelfRayIntersectionVector(const PickRay& ray,
+    const QVector<uint>& jointIndexesToInclude,
+    const QVector<uint>& jointIndexesToDiscard) {
+    RayToAvatarIntersectionResult result;
+    if (QThread::currentThread() != thread()) {
+        BLOCKING_INVOKE_METHOD(const_cast<AvatarManager*>(this), "findSelfRayIntersectionVector",
+            Q_RETURN_ARG(RayToAvatarIntersectionResult, result),
+            Q_ARG(const PickRay&, ray),
+            Q_ARG(const QVector<uint>&, jointIndexesToInclude),
+            Q_ARG(const QVector<uint>&, jointIndexesToDiscard));
+        return result;
+    }
+    glm::vec3 normDirection = glm::normalize(ray.direction);
+    auto jointCollisionResult = _myAvatar->getCharacterController()->rayTest(glmToBullet(ray.origin), glmToBullet(normDirection), 1.0f, jointIndexesToDiscard);
+    if (jointCollisionResult._intersectWithJoint > -1 && jointCollisionResult._distance > 0) {
+        result.intersects = true;
+        result.distance = jointCollisionResult._distance;
+        result.jointIndex = jointCollisionResult._intersectWithJoint;
+        result.avatarID = _myAvatar->getID();
+        result.extraInfo = QVariantMap();
+        result.intersection = jointCollisionResult._intersectionPoint;
+        result.surfaceNormal = jointCollisionResult._intersectionNormal;
+    }
+    return result;
+}
+
 // HACK
 float AvatarManager::getAvatarSortCoefficient(const QString& name) {
     if (name == "size") {

interface/src/avatar/AvatarManager.h

@@ -165,6 +165,28 @@ public:
                                                                                   const QVector<EntityItemID>& avatarsToInclude,
                                                                                   const QVector<EntityItemID>& avatarsToDiscard);
 
+    /**jsdoc
+    * @function AvatarManager.findSelfRayIntersection
+    * @param {PickRay} ray
+    * @param {Uuid[]} [jointsToInclude=[]]
+    * @param {Uuid[]} [jointsToDiscard=[]]
+    * @returns {RayToAvatarIntersectionResult}
+    */
+    Q_INVOKABLE RayToAvatarIntersectionResult findSelfRayIntersection(const PickRay& ray,
+        const QScriptValue& jointIndexesToInclude = QScriptValue(),
+        const QScriptValue& jointIndexesToDiscard = QScriptValue());
+
+    /**jsdoc
+    * @function AvatarManager.findSelfRayIntersectionVector
+    * @param {PickRay} ray
+    * @param {Uuid[]} jointsToInclude
+    * @param {Uuid[]} jointsToDiscard
+    * @returns {RayToAvatarIntersectionResult}
+    */
+    Q_INVOKABLE RayToAvatarIntersectionResult findSelfRayIntersectionVector(const PickRay& ray,
+        const QVector<uint>& jointIndexesToInclude,
+        const QVector<uint>& jointIndexesToDiscard);
+
     /**jsdoc
      * @function AvatarManager.getAvatarSortCoefficient
      * @param {string} name

interface/src/avatar/DetailedMotionState.cpp

@@ -2,8 +2,8 @@
 //  DetailedMotionState.cpp
 //  interface/src/avatar/
 //
-//  Created by Andrew Meadows 2015.05.14
+//  Created by Luis Cuenca 1/11/2019
-//  Copyright 2015 High Fidelity, Inc.
+//  Copyright 2019 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
@@ -14,9 +14,10 @@
 #include <PhysicsCollisionGroups.h>
 #include <PhysicsEngine.h>
 #include <PhysicsHelpers.h>
+#include "MyAvatar.h"
 
 
-DetailedMotionState::DetailedMotionState(OtherAvatarPointer avatar, const btCollisionShape* shape, int jointIndex) : 
+DetailedMotionState::DetailedMotionState(AvatarPointer avatar, const btCollisionShape* shape, int jointIndex) : 
     ObjectMotionState(shape), _avatar(avatar), _jointIndex(jointIndex) {
     assert(_avatar);
     _type = MOTIONSTATE_TYPE_DETAILED;
@@ -56,7 +57,14 @@ PhysicsMotionType DetailedMotionState::computePhysicsMotionType() const {
 
 // virtual and protected
 const btCollisionShape* DetailedMotionState::computeNewShape() {
-    auto shape = _avatar->createDetailedCollisionShapeForJoint(_jointIndex);
+    btCollisionShape* shape = nullptr;
+    if (!_avatar->isMyAvatar()) {
+        OtherAvatarPointer otherAvatar = std::static_pointer_cast<OtherAvatar>(_avatar);
+        shape = otherAvatar->createDetailedCollisionShapeForJoint(_jointIndex);
+    } else {
+        std::shared_ptr<MyAvatar> myAvatar = std::static_pointer_cast<MyAvatar>(_avatar);
+        shape = myAvatar->getCharacterController()->createDetailedCollisionShapeForJoint(_jointIndex);
+    }
     return shape;
 }
 
@@ -160,3 +168,9 @@ void DetailedMotionState::setRigidBody(btRigidBody* body) {
 void DetailedMotionState::setShape(const btCollisionShape* shape) {
     ObjectMotionState::setShape(shape);
 }
+
+void DetailedMotionState::forceActive() {
+    if (_body) {
+        _body->setActivationState(ACTIVE_TAG);
+    }
+}

interface/src/avatar/DetailedMotionState.h

@@ -2,8 +2,8 @@
 //  DetailedMotionState.h
 //  interface/src/avatar/
 //
-//  Created by Andrew Meadows 2015.05.14
+//  Created by Luis Cuenca 1/11/2019
-//  Copyright 2015 High Fidelity, Inc.
+//  Copyright 2019 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
@@ -21,7 +21,7 @@
 
 class DetailedMotionState : public ObjectMotionState {
 public:
-    DetailedMotionState(OtherAvatarPointer avatar, const btCollisionShape* shape, int jointIndex);
+    DetailedMotionState(AvatarPointer avatar, const btCollisionShape* shape, int jointIndex);
 
     virtual void handleEasyChanges(uint32_t& flags) override;
     virtual bool handleHardAndEasyChanges(uint32_t& flags, PhysicsEngine* engine) override;
@@ -67,6 +67,9 @@ public:
     virtual void computeCollisionGroupAndMask(int32_t& group, int32_t& mask) const override;
 
     virtual float getMass() const override;
+    void forceActive();
+    QUuid getAvatarID() { return _avatar->getID(); }
+    int getJointIndex() { return _jointIndex; }
 
     friend class AvatarManager;
     friend class Avatar;
@@ -82,7 +85,7 @@ protected:
     virtual bool isReadyToComputeShape() const override { return true; }
     virtual const btCollisionShape* computeNewShape() override;
 
-    OtherAvatarPointer _avatar;
+    AvatarPointer _avatar;
     float _diameter { 0.0f };
 
     uint32_t _dirtyFlags;

interface/src/avatar/MyAvatar.cpp

@@ -4830,3 +4830,7 @@ void MyAvatar::releaseGrab(const QUuid& grabID) {
         _clientTraitsHandler->markInstancedTraitDeleted(AvatarTraits::Grab, grabID);
     }
 }
+
+std::shared_ptr<MyAvatar> MyAvatar::getSharedMe() {
+    return DependencyManager::get<AvatarManager>()->getMyAvatar();
+}

interface/src/avatar/MyAvatar.h

@@ -37,6 +37,7 @@
 class AvatarActionHold;
 class ModelItemID;
 class MyHead;
+class DetailedMotionState;
 
 enum eyeContactTarget {
     LEFT_EYE,
@@ -1206,6 +1207,8 @@ public:
      */
     Q_INVOKABLE void releaseGrab(const QUuid& grabID);
 
+    std::shared_ptr<MyAvatar> getSharedMe();
+
 public slots:
 
     /**jsdoc

interface/src/avatar/MyCharacterController.cpp

@@ -12,8 +12,10 @@
 #include "MyCharacterController.h"
 
 #include <BulletUtil.h>
+#include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h"
 
 #include "MyAvatar.h"
+#include "DetailedMotionState.h"
 
 // TODO: make avatars stand on steep slope
 // TODO: make avatars not snag on low ceilings
@@ -44,8 +46,8 @@ void MyCharacterController::setDynamicsWorld(btDynamicsWorld* world) {
 void MyCharacterController::updateShapeIfNecessary() {
     if (_pendingFlags & PENDING_FLAG_UPDATE_SHAPE) {
         _pendingFlags &= ~PENDING_FLAG_UPDATE_SHAPE;
-
         if (_radius > 0.0f) {
+            // _pendingFlags |= PENDING_FLAG_RESET_DETAILED_SHAPES;
             // create RigidBody if it doesn't exist
             if (!_rigidBody) {
                 btCollisionShape* shape = computeShape();
@@ -352,3 +354,113 @@ void MyCharacterController::updateMassProperties() {
 
     _rigidBody->setMassProps(mass, inertia);
 }
+
+btCollisionShape* MyCharacterController::createDetailedCollisionShapeForJoint(int jointIndex) {
+    ShapeInfo shapeInfo;
+    _avatar->computeDetailedShapeInfo(shapeInfo, jointIndex);
+    if (shapeInfo.getType() != SHAPE_TYPE_NONE) {
+        btCollisionShape* shape = const_cast<btCollisionShape*>(ObjectMotionState::getShapeManager()->getShape(shapeInfo));
+        return shape;
+    }
+    return nullptr;
+}
+
+DetailedMotionState* MyCharacterController::createDetailedMotionStateForJoint(int jointIndex) {
+    auto shape = createDetailedCollisionShapeForJoint(jointIndex);
+    if (shape) {
+        DetailedMotionState* motionState = new DetailedMotionState(_avatar->getSharedMe(), shape, jointIndex);
+        motionState->setMass(_avatar->computeMass());
+        return motionState;
+    }
+    return nullptr;
+}
+
+void MyCharacterController::resetDetailedMotionStates() {
+    for (size_t i = 0; i < _detailedMotionStates.size(); i++) {
+        _detailedMotionStates[i] = nullptr;
+    }
+    _detailedMotionStates.clear();
+}
+
+void MyCharacterController::buildPhysicsTransaction(PhysicsEngine::Transaction& transaction) {
+    for (size_t i = 0; i < _detailedMotionStates.size(); i++) {
+        _detailedMotionStates[i]->forceActive();
+    }
+    if (_pendingFlags & PENDING_FLAG_REMOVE_DETAILED_FROM_SIMULATION) {
+        _pendingFlags &= ~PENDING_FLAG_REMOVE_DETAILED_FROM_SIMULATION;
+        for (size_t i = 0; i < _detailedMotionStates.size(); i++) {
+            transaction.objectsToRemove.push_back(_detailedMotionStates[i]);
+            _detailedMotionStates[i] = nullptr;
+        }
+        _detailedMotionStates.clear();
+    }
+    if (_pendingFlags & PENDING_FLAG_ADD_DETAILED_TO_SIMULATION) {
+        _pendingFlags &= ~PENDING_FLAG_ADD_DETAILED_TO_SIMULATION;
+        for (int i = 0; i < _avatar->getJointCount(); i++) {
+            auto dMotionState = createDetailedMotionStateForJoint(i);
+            if (dMotionState) {
+                _detailedMotionStates.push_back(dMotionState);
+                transaction.objectsToAdd.push_back(dMotionState);
+            }
+        }
+    } 
+}
+
+void MyCharacterController::handleProcessedPhysicsTransaction(PhysicsEngine::Transaction& transaction) {
+    // things on objectsToRemove are ready for delete
+    for (auto object : transaction.objectsToRemove) {
+        delete object;
+    }
+    transaction.clear();
+}
+
+
+class ClosestDetailed : public btCollisionWorld::AllHitsRayResultCallback {
+public:
+    ClosestDetailed()
+        : btCollisionWorld::AllHitsRayResultCallback(btVector3(0.0f, 0.0f, 0.0f), btVector3(0.0f, 0.0f, 0.0f)) {
+        // the RayResultCallback's group and mask must match MY_AVATAR
+        m_collisionFilterGroup = BULLET_COLLISION_GROUP_DETAILED_RAY;
+        m_collisionFilterMask = BULLET_COLLISION_MASK_DETAILED_RAY;
+    }
+
+    virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult, bool normalInWorldSpace) override {
+        return AllHitsRayResultCallback::addSingleResult(rayResult, normalInWorldSpace);
+    }
+};
+
+MyCharacterController::RayAvatarResult MyCharacterController::rayTest(const btVector3& origin, const btVector3& direction, const btScalar& length,
+                                                                      const QVector<uint>& jointsToExclude) const {
+    RayAvatarResult result;
+    if (_dynamicsWorld) {
+        btVector3 end = origin + length * direction;
+        ClosestDetailed rayCallback = ClosestDetailed();
+        rayCallback.m_flags |= btTriangleRaycastCallback::kF_KeepUnflippedNormal;
+        rayCallback.m_flags |= btTriangleRaycastCallback::kF_UseSubSimplexConvexCastRaytest;
+        _dynamicsWorld->rayTest(origin, end, rayCallback);
+        if (rayCallback.m_hitFractions.size() > 0) {
+            int minIndex = 0;
+            float hitFraction = rayCallback.m_hitFractions[0];
+            for (auto i = 1; i < rayCallback.m_hitFractions.size(); i++) {
+                if (hitFraction > rayCallback.m_hitFractions[i]) {
+                    hitFraction = rayCallback.m_hitFractions[i];
+                    minIndex = i;
+                }
+            }
+            auto object = rayCallback.m_collisionObjects[minIndex];
+            ObjectMotionState* motionState = static_cast<ObjectMotionState*>(object->getUserPointer());
+            if (motionState && motionState->getType() == MOTIONSTATE_TYPE_DETAILED) {
+                DetailedMotionState* detailedMotionState = dynamic_cast<DetailedMotionState*>(motionState);
+                if (detailedMotionState) {
+                    result._intersect = true;
+                    result._intersectWithAvatar = detailedMotionState->getAvatarID();
+                    result._intersectionPoint = bulletToGLM(rayCallback.m_hitPointWorld[minIndex]);
+                    result._intersectionNormal = bulletToGLM(rayCallback.m_hitNormalWorld[minIndex]);
+                    result._distance = length * hitFraction;
+                    result._intersectWithJoint = detailedMotionState->getJointIndex();
+                }
+            }
+        }
+    }
+    return result;
+}

interface/src/avatar/MyCharacterController.h

@@ -15,9 +15,11 @@
 
 #include <CharacterController.h>
 //#include <SharedUtil.h>
+#include <PhysicsEngine.h>
 
 class btCollisionShape;
 class MyAvatar;
+class DetailedMotionState;
 
 class MyCharacterController : public CharacterController {
 public:
@@ -42,6 +44,27 @@ public:
 
     void setDensity(btScalar density) { _density = density; }
 
+    btCollisionShape* createDetailedCollisionShapeForJoint(int jointIndex);
+    DetailedMotionState* createDetailedMotionStateForJoint(int jointIndex);
+    std::vector<DetailedMotionState*>& getDetailedMotionStates() { return _detailedMotionStates; }
+    void clearDetailedMotionStates() { _pendingFlags |= PENDING_FLAG_REMOVE_DETAILED_FROM_SIMULATION; }
+    void resetDetailedMotionStates();
+
+    void buildPhysicsTransaction(PhysicsEngine::Transaction& transaction);
+    void handleProcessedPhysicsTransaction(PhysicsEngine::Transaction& transaction);
+
+
+    struct RayAvatarResult {
+        bool _intersect { false };
+        QUuid _intersectWithAvatar;
+        int _intersectWithJoint { -1 };
+        float _distance { 0.0f };
+        glm::vec3 _intersectionPoint;
+        glm::vec3 _intersectionNormal;
+    };
+    RayAvatarResult rayTest(const btVector3& origin, const btVector3& direction, const btScalar& length,
+                           const QVector<uint>& jointsToExclude) const;
+
 protected:
     void initRayShotgun(const btCollisionWorld* world);
     void updateMassProperties() override;
@@ -56,6 +79,8 @@ protected:
     btAlignedObjectArray<btVector3> _topPoints;
     btAlignedObjectArray<btVector3> _bottomPoints;
     btScalar _density { 1.0f };
+
+    std::vector<DetailedMotionState*> _detailedMotionStates;
 };
 
 #endif // hifi_MyCharacterController_h

interface/src/avatar/OtherAvatar.h

@@ -46,10 +46,11 @@ public:
     bool shouldBeInPhysicsSimulation() const;
     bool needsPhysicsUpdate() const;
 
-    btCollisionShape* OtherAvatar::createDetailedCollisionShapeForJoint(int jointIndex);
+    btCollisionShape* createDetailedCollisionShapeForJoint(int jointIndex);
     DetailedMotionState* createDetailedMotionStateForJoint(std::shared_ptr<OtherAvatar> avatar, int jointIndex);
     std::vector<DetailedMotionState*>& getDetailedMotionStates() { return _detailedMotionStates; }
     void resetDetailedMotionStates();
+
     void updateCollisionGroup(bool myAvatarCollide);
 
     friend AvatarManager;
@@ -64,5 +65,6 @@ protected:
 };
 
 using OtherAvatarPointer = std::shared_ptr<OtherAvatar>;
+using AvatarPointer = std::shared_ptr<Avatar>;
 
 #endif  // hifi_OtherAvatar_h

libraries/avatars-renderer/src/avatars-renderer/Avatar.cpp

@@ -1671,7 +1671,7 @@ void Avatar::rigReset() {
 
 void Avatar::computeMultiSphereShapes() {
     const Rig& rig = getSkeletonModel()->getRig();
-    glm::vec3 scale = extractScale(rig.getGeometryToRigTransform());
+    glm::vec3 scale = extractScale(rig.getGeometryOffsetPose());
     const HFMModel& geometry = getSkeletonModel()->getHFMModel();
     int jointCount = rig.getJointStateCount();
     _multiSphereShapes.clear();
@@ -1691,8 +1691,8 @@ void Avatar::computeMultiSphereShapes() {
         MultiSphereShape multiSphereShape;
         if (multiSphereShape.computeMultiSphereShape(jointName, btPoints)) {
             multiSphereShape.calculateDebugLines();
-            multiSphereShape.setScale(getTargetScale());
+            multiSphereShape.setScale(_targetScale);
-;        }
+        }
         _multiSphereShapes.push_back(multiSphereShape);
     }
 }

libraries/avatars/src/AvatarData.cpp

@@ -2898,6 +2898,7 @@ QScriptValue RayToAvatarIntersectionResultToScriptValue(QScriptEngine* engine, c
     obj.setProperty("intersection", intersection);
     QScriptValue surfaceNormal = vec3ToScriptValue(engine, value.surfaceNormal);
     obj.setProperty("surfaceNormal", surfaceNormal);
+    obj.setProperty("jointIndex", value.jointIndex);
     obj.setProperty("extraInfo", engine->toScriptValue(value.extraInfo));
     return obj;
 }
@@ -2917,6 +2918,7 @@ void RayToAvatarIntersectionResultFromScriptValue(const QScriptValue& object, Ra
     if (surfaceNormal.isValid()) {
         vec3FromScriptValue(surfaceNormal, value.surfaceNormal);
     }
+    value.jointIndex = object.property("jointIndex").toInt32();
     value.extraInfo = object.property("extraInfo").toVariant().toMap();
 }
 

libraries/avatars/src/AvatarData.h

@@ -1614,6 +1614,7 @@ public:
     BoxFace face;
     glm::vec3 intersection;
     glm::vec3 surfaceNormal;
+    int jointIndex { -1 };
     QVariantMap extraInfo;
 };
 Q_DECLARE_METATYPE(RayToAvatarIntersectionResult)

libraries/physics/src/CharacterController.cpp

@@ -137,7 +137,8 @@ void CharacterController::setDynamicsWorld(btDynamicsWorld* world) {
     if (_dynamicsWorld) {
         if (_pendingFlags & PENDING_FLAG_UPDATE_SHAPE) {
             // shouldn't fall in here, but if we do make sure both ADD and REMOVE bits are still set
-            _pendingFlags |= PENDING_FLAG_ADD_TO_SIMULATION | PENDING_FLAG_REMOVE_FROM_SIMULATION;
+            _pendingFlags |= PENDING_FLAG_ADD_TO_SIMULATION | PENDING_FLAG_REMOVE_FROM_SIMULATION | 
+                             PENDING_FLAG_ADD_DETAILED_TO_SIMULATION | PENDING_FLAG_REMOVE_DETAILED_FROM_SIMULATION;
         } else {
             _pendingFlags &= ~PENDING_FLAG_ADD_TO_SIMULATION;
         }
@@ -445,10 +446,10 @@ void CharacterController::setLocalBoundingBox(const glm::vec3& minCorner, const
 
         if (_dynamicsWorld) {
             // must REMOVE from world prior to shape update
-            _pendingFlags |= PENDING_FLAG_REMOVE_FROM_SIMULATION;
+            _pendingFlags |= PENDING_FLAG_REMOVE_FROM_SIMULATION | PENDING_FLAG_REMOVE_DETAILED_FROM_SIMULATION;
         }
         _pendingFlags |= PENDING_FLAG_UPDATE_SHAPE;
-        _pendingFlags |= PENDING_FLAG_ADD_TO_SIMULATION;
+        _pendingFlags |= PENDING_FLAG_ADD_TO_SIMULATION | PENDING_FLAG_ADD_DETAILED_TO_SIMULATION;
     }
 
     // it's ok to change offset immediately -- there are no thread safety issues here

libraries/physics/src/CharacterController.h

@@ -32,6 +32,9 @@ const uint32_t PENDING_FLAG_UPDATE_SHAPE = 1U << 2;
 const uint32_t PENDING_FLAG_JUMP = 1U << 3;
 const uint32_t PENDING_FLAG_UPDATE_COLLISION_GROUP = 1U << 4;
 const uint32_t PENDING_FLAG_RECOMPUTE_FLYING = 1U << 5;
+const uint32_t PENDING_FLAG_ADD_DETAILED_TO_SIMULATION = 1U << 6;
+const uint32_t PENDING_FLAG_REMOVE_DETAILED_FROM_SIMULATION = 1U << 7;
+
 const float DEFAULT_MIN_FLOOR_NORMAL_DOT_UP = cosf(PI / 3.0f);
 
 class btRigidBody;

libraries/physics/src/MultiSphereShape.cpp

@@ -254,6 +254,7 @@ bool MultiSphereShape::computeMultiSphereShape(const QString& name, const std::v
     for (size_t i = 0; i < _spheres.size(); i++) {
         _spheres[i]._position += _midPoint;
     }
+    
     return _mode != CollisionShapeExtractionMode::None;
 }