Refactor and optimizations

interface/src/Application.cpp

@@ -2639,7 +2639,7 @@ Application::~Application() {
     
     auto myCharacterController = getMyAvatar()->getCharacterController();
     myCharacterController->clearDetailedMotionStates();
-
+    
     myCharacterController->buildPhysicsTransaction(transaction);
     _physicsEngine->processTransaction(transaction);
     myCharacterController->handleProcessedPhysicsTransaction(transaction);

interface/src/avatar/AvatarManager.cpp

@@ -407,6 +407,7 @@ void AvatarManager::buildPhysicsTransaction(PhysicsEngine::Transaction& transact
             if (isInPhysics) {
                 transaction.objectsToRemove.push_back(avatar->_motionState);
                 avatar->_motionState = nullptr;
+                
                 auto& detailedMotionStates = avatar->getDetailedMotionStates();
                 for (auto& mState : detailedMotionStates) {
                     if (mState) {
@@ -415,37 +416,39 @@ void AvatarManager::buildPhysicsTransaction(PhysicsEngine::Transaction& transact
                 }
                 qDebug() << "Removing " << detailedMotionStates.size() << " detailed motion states from " << avatar->getSessionUUID();
                 avatar->resetDetailedMotionStates();
+                
             } else {
-                if (avatar->_motionState == nullptr) {
+                ShapeInfo shapeInfo;
-                    ShapeInfo shapeInfo;
+                avatar->computeShapeInfo(shapeInfo);
-                    avatar->computeShapeInfo(shapeInfo);
+                btCollisionShape* shape = const_cast<btCollisionShape*>(ObjectMotionState::getShapeManager()->getShape(shapeInfo));
-                    btCollisionShape* shape = const_cast<btCollisionShape*>(ObjectMotionState::getShapeManager()->getShape(shapeInfo));
+                if (shape) {
-                    if (shape) {
+                    AvatarMotionState* motionState = new AvatarMotionState(avatar, shape);
-                        AvatarMotionState* motionState = new AvatarMotionState(avatar, shape);
+                    motionState->setMass(avatar->computeMass());
-                        motionState->setMass(avatar->computeMass());
+                    avatar->_motionState = motionState;
-                        avatar->_motionState = motionState;
+                    transaction.objectsToAdd.push_back(motionState);
-                        transaction.objectsToAdd.push_back(motionState);
+                } else {
-                    }
+                    failedShapeBuilds.insert(avatar);
                 }
+
                 if (avatar->getDetailedMotionStates().size() == 0) {
                     avatar->createDetailedMotionStates(avatar);
                     for (auto dMotionState : avatar->getDetailedMotionStates()) {
                         transaction.objectsToAdd.push_back(dMotionState);
                     }
-                    if (avatar->_motionState == nullptr || avatar->getDetailedMotionStates().size() == 0) {
-                        failedShapeBuilds.insert(avatar);
-                    }
                 }
+                
                 qDebug() << "Adding " << avatar->getDetailedMotionStates().size() << " detailed motion states from " << avatar->getSessionUUID();
             }
         } else if (isInPhysics) {
             transaction.objectsToChange.push_back(avatar->_motionState);
+            
             auto& detailedMotionStates = avatar->getDetailedMotionStates();
             for (auto& mState : detailedMotionStates) {
                 if (mState) {
                     transaction.objectsToChange.push_back(mState);
                 }
             }
+            
         }
     }
     _avatarsToChangeInPhysics.swap(failedShapeBuilds);
@@ -656,21 +659,80 @@ RayToAvatarIntersectionResult AvatarManager::findRayIntersectionVector(const Pic
                                   Q_ARG(const QVector<EntityItemID>&, avatarsToDiscard));
         return result;
     }
+    
+    glm::vec3 rayDirectionInv = { ray.direction.x != 0 ? 1.0f / ray.direction.x : INFINITY,
+        ray.direction.y != 0 ? 1.0f / ray.direction.y : INFINITY,
+        ray.direction.z != 0 ? 1.0f / ray.direction.z : INFINITY };
 
     float distance = (float)INT_MAX;  // with FLT_MAX bullet rayTest does not return results 
     BoxFace face = BoxFace::UNKNOWN_FACE;
     glm::vec3 surfaceNormal;
     QVariantMap extraInfo;
-    MyCharacterController::RayAvatarResult physicsResult = _myAvatar->getCharacterController()->rayTest(glmToBullet(ray.origin), glmToBullet(ray.direction), distance, QVector<uint>());
+    std::vector<MyCharacterController::RayAvatarResult> physicsResults = _myAvatar->getCharacterController()->rayTest(glmToBullet(ray.origin), glmToBullet(ray.direction), distance, QVector<uint>());
-    if (physicsResult._intersect) {
+
-        result.intersects = true;
+    if (physicsResults.size() > 0) {
-        result.avatarID = physicsResult._intersectWithAvatar;
+        MyCharacterController::RayAvatarResult rayAvatarResult;
-        result.distance = physicsResult._distance;
+        for (auto &hit : physicsResults) {
-        result.surfaceNormal = physicsResult._intersectionNormal;
+            if ((avatarsToInclude.size() > 0 && !avatarsToInclude.contains(hit._intersectWithAvatar)) ||
-        result.jointIndex = physicsResult._intersectWithJoint;
+                (avatarsToDiscard.size() > 0 && avatarsToDiscard.contains(hit._intersectWithAvatar))) {
-        result.intersection = physicsResult._intersectionPoint;
+                continue;
-        result.extraInfo = extraInfo;
+            }
-        result.face = face;
+            if (!hit._isBound) {
+                rayAvatarResult = hit;
+                break;
+            } else {
+                auto avatarMap = getHashCopy();
+                auto avatarID = hit._intersectWithAvatar;
+                AvatarHash::iterator itr = avatarMap.find(avatarID);
+                if (itr != avatarMap.end()) {
+                    const auto& avatar = std::static_pointer_cast<Avatar>(*itr);
+                    auto &multiSpheres = avatar->getMultiSphereShapes();
+                    if (multiSpheres.size() > 0) {
+                        std::vector<MyCharacterController::RayAvatarResult> boxHits;
+                        for (auto i = 0; i < hit._boundJoints.size(); i++) {
+                            assert(hit._boundJoints[i] < multiSpheres.size());
+                            auto &mSphere = multiSpheres[hit._boundJoints[i]];
+                            if (mSphere.isValid()) {
+                                float boundDistance = FLT_MAX;
+                                BoxFace face;
+                                glm::vec3 surfaceNormal;
+                                auto &bbox = mSphere.getBoundingBox();
+                                if (bbox.findRayIntersection(ray.origin, ray.direction, rayDirectionInv, boundDistance, face, surfaceNormal)) {
+                                    MyCharacterController::RayAvatarResult boxHit;
+                                    boxHit._distance = boundDistance;
+                                    boxHit._intersect = true;
+                                    boxHit._intersectionNormal = surfaceNormal;
+                                    boxHit._intersectionPoint = ray.origin + boundDistance * glm::normalize(ray.direction);
+                                    boxHit._intersectWithAvatar = avatarID;
+                                    boxHit._intersectWithJoint = mSphere.getJointIndex();
+                                    boxHits.push_back(boxHit);
+                                }
+                            }
+                        }
+                        if (boxHits.size() > 0) {
+                            if (boxHits.size() > 1) {
+                                std::sort(boxHits.begin(), boxHits.end(), [](const MyCharacterController::RayAvatarResult& hitA,
+                                                                             const MyCharacterController::RayAvatarResult& hitB) {
+                                    return hitA._distance < hitB._distance;
+                                });
+                            } 
+                            rayAvatarResult = boxHits[0];
+                            break;
+                        }
+                    }
+                }     
+            }
+        }
+        if (rayAvatarResult._intersect) {
+            result.intersects = true;
+            result.avatarID = rayAvatarResult._intersectWithAvatar;
+            result.distance = rayAvatarResult._distance;
+            result.surfaceNormal = rayAvatarResult._intersectionNormal;
+            result.jointIndex = rayAvatarResult._intersectWithJoint;
+            result.intersection = rayAvatarResult._intersectionPoint;
+            result.extraInfo = extraInfo;
+            result.face = face;
+        }
     }
     return result;
 }
@@ -769,42 +831,6 @@ 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,28 +165,6 @@ 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

@@ -20,6 +20,7 @@
 DetailedMotionState::DetailedMotionState(AvatarPointer avatar, const btCollisionShape* shape, int jointIndex) : 
     ObjectMotionState(shape), _avatar(avatar), _jointIndex(jointIndex) {
     assert(_avatar);
+    _otherAvatar = std::static_pointer_cast<OtherAvatar>(_avatar);
     _type = MOTIONSTATE_TYPE_DETAILED;
 }
 
@@ -59,13 +60,8 @@ PhysicsMotionType DetailedMotionState::computePhysicsMotionType() const {
 const btCollisionShape* DetailedMotionState::computeNewShape() {
     btCollisionShape* shape = nullptr;
     if (!_avatar->isMyAvatar()) {
-        OtherAvatarPointer otherAvatar = std::static_pointer_cast<OtherAvatar>(_avatar);
+        if (_otherAvatar) {
-        if (otherAvatar) {
+            shape =  _otherAvatar->createCollisionShape(_jointIndex, _isBound, _boundJoints);
-            if (_isAvatarCapsule) {
-                shape = otherAvatar->createCapsuleCollisionShape();
-            } else {
-                shape = otherAvatar->createDetailedCollisionShapeForJoint(_jointIndex);
-            }   
         }
     } else {
         std::shared_ptr<MyAvatar> myAvatar = std::static_pointer_cast<MyAvatar>(_avatar);
@@ -115,7 +111,8 @@ float DetailedMotionState::getObjectAngularDamping() const {
 
 // virtual
 glm::vec3 DetailedMotionState::getObjectPosition() const {
-    return _isAvatarCapsule ? _avatar->getFitBounds().calcCenter() : _avatar->getJointPosition(_jointIndex);
+    auto bodyLOD = _otherAvatar->getBodyLOD();
+    return bodyLOD == OtherAvatar::BodyLOD::Sphere ? _avatar->getFitBounds().calcCenter() : _avatar->getJointPosition(_jointIndex);
 }
 
 // virtual

interface/src/avatar/DetailedMotionState.h

@@ -68,9 +68,10 @@ public:
 
     virtual float getMass() const override;
     void forceActive();
-    QUuid getAvatarID() { return _avatar->getID(); }
+    QUuid getAvatarID() const { return _avatar->getID(); }
-    int getJointIndex() { return _jointIndex; }
+    int getJointIndex() const { return _jointIndex; }
-    void setIsAvatarCapsule(bool isAvatarCapsule) { _isAvatarCapsule = isAvatarCapsule; }
+    void setIsBound(bool isBound, std::vector<int> boundJoints) { _isBound = isBound; _boundJoints = boundJoints; }
+    bool getIsBound(std::vector<int>& boundJoints) const { boundJoints = _boundJoints; return _isBound; }
 
     friend class AvatarManager;
     friend class Avatar;
@@ -91,8 +92,9 @@ protected:
 
     uint32_t _dirtyFlags;
     int _jointIndex { -1 };
-
+    OtherAvatarPointer _otherAvatar { nullptr };
-    bool _isAvatarCapsule { false };
+    bool _isBound { false };
+    std::vector<int> _boundJoints;
 };
 
 #endif // hifi_DetailedMotionState_h

interface/src/avatar/MyAvatar.cpp

@@ -2606,15 +2606,16 @@ void MyAvatar::postUpdate(float deltaTime, const render::ScenePointer& scene) {
         if (_skeletonModel && _skeletonModel->isLoaded()) {
             const Rig& rig = _skeletonModel->getRig();
             const HFMModel& hfmModel = _skeletonModel->getHFMModel();
-            for (int i = 0; i < rig.getJointStateCount(); i++) {
+            int jointCount = rig.getJointStateCount();
-                AnimPose jointPose;
+            if (jointCount == _multiSphereShapes.size()) {
-                rig.getAbsoluteJointPoseInRigFrame(i, jointPose);
+                int count = 0;
-                const HFMJointShapeInfo& shapeInfo = hfmModel.joints[i].shapeInfo;
+                for (int i = 0; i < jointCount; i++) {
-                const AnimPose pose = rigToWorldPose * jointPose;
+                    AnimPose jointPose;
-                for (size_t j = 0; j < shapeInfo.debugLines.size() / 2; j++) {
+                    rig.getAbsoluteJointPoseInRigFrame(i, jointPose);
-                    glm::vec3 pointA = pose.xformPoint(shapeInfo.debugLines[2 * j]);
+                    const AnimPose pose = rigToWorldPose * jointPose;
-                    glm::vec3 pointB = pose.xformPoint(shapeInfo.debugLines[2 * j + 1]);
+                    auto &multiSphere = _multiSphereShapes[i];
-                    DebugDraw::getInstance().drawRay(pointA, pointB, DEBUG_COLORS[i % NUM_DEBUG_COLORS]);
+                    auto debugLines = multiSphere.getDebugLines();
+                    DebugDraw::getInstance().drawRays(debugLines, DEBUG_COLORS[count++ % NUM_DEBUG_COLORS], pose.trans(), pose.rot());
                 }
             }
         }

interface/src/avatar/MyCharacterController.cpp

@@ -429,9 +429,9 @@ public:
     }
 };
 
-MyCharacterController::RayAvatarResult MyCharacterController::rayTest(const btVector3& origin, const btVector3& direction, const btScalar& length,
+std::vector<MyCharacterController::RayAvatarResult> MyCharacterController::rayTest(const btVector3& origin, const btVector3& direction,
-                                                                      const QVector<uint>& jointsToExclude) const {
+                                                                                   const btScalar& length, const QVector<uint>& jointsToExclude) const {
-    RayAvatarResult result;
+    std::vector<RayAvatarResult> foundAvatars;
     if (_dynamicsWorld) {
         btVector3 end = origin + length * direction;
         ClosestDetailed rayCallback = ClosestDetailed();
@@ -439,28 +439,26 @@ MyCharacterController::RayAvatarResult MyCharacterController::rayTest(const btVe
         rayCallback.m_flags |= btTriangleRaycastCallback::kF_UseSubSimplexConvexCastRaytest;
         _dynamicsWorld->rayTest(origin, end, rayCallback);
         if (rayCallback.m_hitFractions.size() > 0) {
-            int minIndex = 0;
+            for (int i = 0; i < rayCallback.m_hitFractions.size(); i++) {
-            float hitFraction = rayCallback.m_hitFractions[0];
+                auto object = rayCallback.m_collisionObjects[i];
-            for (auto i = 1; i < rayCallback.m_hitFractions.size(); i++) {
+                ObjectMotionState* motionState = static_cast<ObjectMotionState*>(object->getUserPointer());
-                if (hitFraction > rayCallback.m_hitFractions[i]) {
+                if (motionState && motionState->getType() == MOTIONSTATE_TYPE_DETAILED) {
-                    hitFraction = rayCallback.m_hitFractions[i];
+                    DetailedMotionState* detailedMotionState = dynamic_cast<DetailedMotionState*>(motionState);
-                    minIndex = i;
+                    MyCharacterController::RayAvatarResult result;
-                }
-            }
-            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._intersectionPoint = bulletToGLM(rayCallback.m_hitPointWorld[i]);
-                    result._intersectionNormal = bulletToGLM(rayCallback.m_hitNormalWorld[minIndex]);
+                    result._intersectionNormal = bulletToGLM(rayCallback.m_hitNormalWorld[i]);
-                    result._distance = length * hitFraction;
+                    result._distance = length * rayCallback.m_hitFractions[i];
                     result._intersectWithJoint = detailedMotionState->getJointIndex();
+                    result._isBound = detailedMotionState->getIsBound(result._boundJoints);
+                    foundAvatars.push_back(result);
                 }
             }
+            std::sort(foundAvatars.begin(), foundAvatars.end(), [](const RayAvatarResult& resultA, const RayAvatarResult& resultB) {
+                return resultA._distance < resultB._distance;
+            });
         }
     }
-    return result;
+    return foundAvatars;
 }

interface/src/avatar/MyCharacterController.h

@@ -56,14 +56,16 @@ public:
 
     struct RayAvatarResult {
         bool _intersect { false };
+        bool _isBound { false };
         QUuid _intersectWithAvatar;
         int _intersectWithJoint { -1 };
         float _distance { 0.0f };
         glm::vec3 _intersectionPoint;
         glm::vec3 _intersectionNormal;
+        std::vector<int> _boundJoints;
     };
-    RayAvatarResult rayTest(const btVector3& origin, const btVector3& direction, const btScalar& length,
+    std::vector<RayAvatarResult> rayTest(const btVector3& origin, const btVector3& direction, const btScalar& length,
-                           const QVector<uint>& jointsToExclude) const;
+                                         const QVector<uint>& jointsToExclude) const;
 
 protected:
     void initRayShotgun(const btCollisionWorld* world);

interface/src/avatar/OtherAvatar.cpp

@@ -112,43 +112,62 @@ int OtherAvatar::parseDataFromBuffer(const QByteArray& buffer) {
     return bytesRead;
 }
 
-btCollisionShape* OtherAvatar::createDetailedCollisionShapeForJoint(int jointIndex) {
+btCollisionShape* OtherAvatar::createCollisionShape(int jointIndex, bool& isBound, std::vector<int>& boundJoints) {
     ShapeInfo shapeInfo;
-    computeDetailedShapeInfo(shapeInfo, jointIndex);
+    isBound = false;
+    auto jointName = jointIndex > -1 && jointIndex < _multiSphereShapes.size() ? _multiSphereShapes[jointIndex].getJointName() : ""; 
+    switch (_bodyLOD) {
+    case BodyLOD::Sphere:
+        shapeInfo.setSphere(0.5f * getFitBounds().getDimensions().y);
+        boundJoints.clear();
+        for (auto &spheres : _multiSphereShapes) {
+            if (spheres.isValid()) {
+                boundJoints.push_back(spheres.getJointIndex());
+            }
+        }
+        isBound = true;
+        break;
+    case BodyLOD::MultiSphereLow:
+        if (jointName.contains("RightHand", Qt::CaseInsensitive) || jointName.contains("LeftHand", Qt::CaseInsensitive))  {
+            if (jointName.size() <= QString("RightHand").size()) {
+                AABox handBound;
+                for (auto &spheres : _multiSphereShapes) {
+                    if (spheres.isValid() && spheres.getJointName().contains(jointName, Qt::CaseInsensitive)) {
+                        boundJoints.push_back(spheres.getJointIndex());
+                        handBound += spheres.getBoundingBox();
+                    }
+                }
+                shapeInfo.setSphere(0.5f * handBound.getLargestDimension());
+                glm::vec3 jointPosition;
+                glm::quat jointRotation;
+                _skeletonModel->getJointPositionInWorldFrame(jointIndex, jointPosition);
+                _skeletonModel->getJointRotationInWorldFrame(jointIndex, jointRotation);
+                glm::vec3 positionOffset = glm::inverse(jointRotation) * (handBound.calcCenter() - jointPosition);
+                shapeInfo.setOffset(positionOffset);
+                isBound = true;
+            }
+            break;
+        }
+    case BodyLOD::MultiSphereHigh:
+        computeDetailedShapeInfo(shapeInfo, jointIndex);
+        break;
+    default:
+        break;
+    }
     if (shapeInfo.getType() != SHAPE_TYPE_NONE) {
-        btCollisionShape* shape = const_cast<btCollisionShape*>(ObjectMotionState::getShapeManager()->getShape(shapeInfo));
+        return const_cast<btCollisionShape*>(ObjectMotionState::getShapeManager()->getShape(shapeInfo));
-        return shape;
     }
     return nullptr;
 }
 
-btCollisionShape* OtherAvatar::createCapsuleCollisionShape() {
+DetailedMotionState* OtherAvatar::createMotionState(std::shared_ptr<OtherAvatar> avatar, int jointIndex) {
-    ShapeInfo shapeInfo;
+    bool isBound = false;
-    computeShapeInfo(shapeInfo);
+    std::vector<int> boundJoints;
-    shapeInfo.setOffset(glm::vec3(0.0f));
+    btCollisionShape* shape = createCollisionShape(jointIndex, isBound, boundJoints);
-    if (shapeInfo.getType() != SHAPE_TYPE_NONE) {
-        btCollisionShape* shape = const_cast<btCollisionShape*>(ObjectMotionState::getShapeManager()->getShape(shapeInfo));
-        return shape;
-    }
-    return nullptr;
-}
-
-DetailedMotionState* OtherAvatar::createDetailedMotionStateForJoint(std::shared_ptr<OtherAvatar> avatar, int jointIndex) {
-    auto shape = createDetailedCollisionShapeForJoint(jointIndex);
     if (shape) {
         DetailedMotionState* motionState = new DetailedMotionState(avatar, shape, jointIndex);
         motionState->setMass(computeMass());
-        return motionState;
+        motionState->setIsBound(isBound, boundJoints);
-    }
-    return nullptr;
-}
-
-DetailedMotionState* OtherAvatar::createCapsuleMotionState(std::shared_ptr<OtherAvatar> avatar) {
-    auto shape = createCapsuleCollisionShape();
-    if (shape) {
-        DetailedMotionState* motionState = new DetailedMotionState(avatar, shape, -1);
-        motionState->setIsAvatarCapsule(true);
-        motionState->setMass(computeMass());
         return motionState;
     }
     return nullptr;
@@ -178,11 +197,27 @@ void OtherAvatar::setWorkloadRegion(uint8_t region) {
 }
 
 void OtherAvatar::computeShapeLOD() {
-    auto newBodyLOD = (_workloadRegion < workload::Region::R3 && !isDead()) ? BodyLOD::MultiSphereShapes : BodyLOD::CapsuleShape;
+    // auto newBodyLOD = _workloadRegion < workload::Region::R3 ? BodyLOD::MultiSphereShapes : BodyLOD::CapsuleShape;
-    if (newBodyLOD != _bodyLOD) {
+    // auto newBodyLOD = BodyLOD::CapsuleShape;
-        _bodyLOD = newBodyLOD;
+    BodyLOD newLOD;
+    switch (_workloadRegion) {
+    case workload::Region::R1:
+        newLOD = BodyLOD::MultiSphereHigh;
+        break;
+    case workload::Region::R2:
+        newLOD = BodyLOD::MultiSphereLow;
+        break;
+    case workload::Region::UNKNOWN:
+    case workload::Region::INVALID:
+    case workload::Region::R3:
+    default:
+        newLOD = BodyLOD::Sphere;
+        break;
+    }
+    if (newLOD != _bodyLOD) {
+        _bodyLOD = newLOD;
         if (isInPhysicsSimulation()) {
-            qDebug() << "Changing to body LOD " << (_bodyLOD == BodyLOD::MultiSphereShapes ? "MultiSpheres" : "Capsule");
+            qDebug() << "Changing to body LOD " << newLOD;
             _needsReinsertion = true;
         }
     }
@@ -193,7 +228,7 @@ bool OtherAvatar::isInPhysicsSimulation() const {
 }
 
 bool OtherAvatar::shouldBeInPhysicsSimulation() const {
-    return !(isInPhysicsSimulation() && _needsReinsertion);
+    return !isDead() && !(isInPhysicsSimulation() && _needsReinsertion);
 }
 
 bool OtherAvatar::needsPhysicsUpdate() const {
@@ -228,18 +263,18 @@ void OtherAvatar::updateCollisionGroup(bool myAvatarCollide) {
 
 void OtherAvatar::createDetailedMotionStates(const std::shared_ptr<OtherAvatar>& avatar){
     auto& detailedMotionStates = getDetailedMotionStates();
-    if (_bodyLOD == BodyLOD::MultiSphereShapes) {
+    if (_bodyLOD == BodyLOD::Sphere) {
+        auto dMotionState = createMotionState(avatar, -1);
+        if (dMotionState) {
+            detailedMotionStates.push_back(dMotionState);
+        }
+    } else {
         for (int i = 0; i < getJointCount(); i++) {
-            auto dMotionState = createDetailedMotionStateForJoint(avatar, i);
+            auto dMotionState = createMotionState(avatar, i);
             if (dMotionState) {
                 detailedMotionStates.push_back(dMotionState);
             }
         }
-    } else if (_bodyLOD == BodyLOD::CapsuleShape) {
-        auto dMotionState = createCapsuleMotionState(avatar);
-        if (dMotionState) {
-            detailedMotionStates.push_back(dMotionState);
-        }
     }
     _needsReinsertion = false;
 }

interface/src/avatar/OtherAvatar.h

@@ -28,8 +28,9 @@ public:
     virtual ~OtherAvatar();
 
     enum BodyLOD {
-        CapsuleShape,
+        Sphere = 0,
-        MultiSphereShapes
+        MultiSphereLow, // No finger joints
+        MultiSphereHigh // All joints
     };
 
     virtual void instantiableAvatar() override { };
@@ -51,10 +52,8 @@ public:
     bool shouldBeInPhysicsSimulation() const;
     bool needsPhysicsUpdate() const;
 
-    btCollisionShape* createDetailedCollisionShapeForJoint(int jointIndex);
+    btCollisionShape* createCollisionShape(int jointIndex, bool& isBound, std::vector<int>& boundJoints);
-    btCollisionShape* createCapsuleCollisionShape();
+    DetailedMotionState* createMotionState(std::shared_ptr<OtherAvatar> avatar, int jointIndex);
-    DetailedMotionState* createDetailedMotionStateForJoint(std::shared_ptr<OtherAvatar> avatar, int jointIndex);
-    DetailedMotionState* createCapsuleMotionState(std::shared_ptr<OtherAvatar> avatar);
     void createDetailedMotionStates(const std::shared_ptr<OtherAvatar>& avatar);
     std::vector<DetailedMotionState*>& getDetailedMotionStates() { return _detailedMotionStates; }
     void resetDetailedMotionStates();
@@ -72,7 +71,7 @@ protected:
     std::vector<DetailedMotionState*> _detailedMotionStates;
     int32_t _spaceIndex { -1 };
     uint8_t _workloadRegion { workload::Region::INVALID };
-    BodyLOD _bodyLOD { BodyLOD::CapsuleShape };
+    BodyLOD _bodyLOD { BodyLOD::Sphere };
     bool _needsReinsertion { false };
 };
 

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

@@ -1689,7 +1689,7 @@ void Avatar::computeMultiSphereShapes() {
         }
         auto jointName = rig.nameOfJoint(i).toUpper();
         MultiSphereShape multiSphereShape;
-        if (multiSphereShape.computeMultiSphereShape(jointName, btPoints)) {
+        if (multiSphereShape.computeMultiSphereShape(i, jointName, btPoints)) {
             multiSphereShape.calculateDebugLines();
             multiSphereShape.setScale(_targetScale);
         }

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

@@ -320,6 +320,7 @@ public:
 
     virtual void computeShapeInfo(ShapeInfo& shapeInfo);
     virtual void computeDetailedShapeInfo(ShapeInfo& shapeInfo, int jointIndex);
+
     void getCapsule(glm::vec3& start, glm::vec3& end, float& radius);
     float computeMass();
     /**jsdoc

libraries/physics/src/MultiSphereShape.cpp

@@ -126,9 +126,11 @@ void MultiSphereShape::filterUniquePoints(const std::vector<btVector3>& kdop, st
     }
 }
 
-bool MultiSphereShape::computeMultiSphereShape(const QString& name, const std::vector<btVector3>& kdop, float scale) {
+bool MultiSphereShape::computeMultiSphereShape(int jointIndex, const QString& name, const std::vector<btVector3>& kdop, float scale) {
     _scale = scale;
-    _mode = getExtractionModeByName(name);
+    _jointIndex = jointIndex;
+    _name = name;
+    _mode = getExtractionModeByName(_name);
     if (_mode == CollisionShapeExtractionMode::None || kdop.size() < 4 || kdop.size() > 200) {
         return false;
     }

libraries/physics/src/MultiSphereShape.h

@@ -74,12 +74,16 @@ const std::vector<glm::vec3> CORNER_SIGNS = {
 class MultiSphereShape {
 public:
     MultiSphereShape() {};
-    bool computeMultiSphereShape(const QString& name, const std::vector<btVector3>& points, float scale = 1.0f);
+    bool computeMultiSphereShape(int jointIndex, const QString& name, const std::vector<btVector3>& points, float scale = 1.0f);
     void calculateDebugLines();
     const std::vector<SphereShapeData>& getSpheresData() const { return _spheres; }
     const std::vector<std::pair<glm::vec3, glm::vec3>>& getDebugLines() const { return _debugLines; }
     void setScale(float scale);
     AABox& updateBoundingBox(const glm::vec3& position, const glm::quat& rotation);
+    const AABox& getBoundingBox() const { return _boundingBox; }
+    int getJointIndex() const { return _jointIndex; }
+    QString getJointName() const { return _name; }
+    bool isValid() const { return _spheres.size() > 0; }
 
 private:
     CollisionShapeExtractionMode getExtractionModeByName(const QString& name);
@@ -94,6 +98,9 @@ private:
     void connectEdges(std::vector<std::pair<glm::vec3, glm::vec3>>& outLines, const std::vector<glm::vec3>& edge1, 
                       const std::vector<glm::vec3>& edge2, bool reverse = false);
     void connectSpheres(int index1, int index2, bool onlyEdges = false);
+
+    int _jointIndex { -1 };
+    QString _name;
     std::vector<SphereShapeData> _spheres;
     std::vector<std::pair<glm::vec3, glm::vec3>> _debugLines;
     CollisionShapeExtractionMode _mode;

libraries/physics/src/PhysicsEngine.cpp

@@ -328,6 +328,9 @@ void PhysicsEngine::processTransaction(PhysicsEngine::Transaction& transaction)
         if (object->getMotionType() == MOTION_TYPE_STATIC && object->isActive()) {
             _activeStaticBodies.insert(object->getRigidBody());
         }
+        if (object->getType() == MOTIONSTATE_TYPE_AVATAR) {
+            _dynamicsWorld->updateSingleAabb(object->_body);
+        }        
     }
     // activeStaticBodies have changed (in an Easy way) and need their Aabbs updated
     // but we've configured Bullet to NOT update them automatically (for improved performance)

libraries/physics/src/ThreadSafeDynamicsWorld.cpp

@@ -172,3 +172,45 @@ void ThreadSafeDynamicsWorld::saveKinematicState(btScalar timeStep) {
         }
     }
 }
+
+void ThreadSafeDynamicsWorld::drawConnectedSpheres(btIDebugDraw* drawer, btScalar radius1, btScalar radius2, const btVector3& position1, const btVector3& position2, const btVector3& color) {
+    int stepDegrees = 30;
+    btVector3 direction = position2 - position1;
+    btVector3 xAxis = direction.cross(btVector3(0.0f, 1.0f, 0.0f));
+    xAxis = xAxis.length() < EPSILON ? btVector3(1.0f, 0.0f, 0.0f) : xAxis.normalize();
+    btVector3 zAxis = xAxis.cross(btVector3(0.0f, 1.0f, 0.0f));
+    zAxis = (direction.normalize().getY() < EPSILON) ? btVector3(0.0f, 1.0f, 0.0f) : zAxis.normalize();
+    for (int i = 0; i < 360; i += stepDegrees) {
+        float x1 = btSin(btScalar(i)*SIMD_RADS_PER_DEG) * radius1;
+        float z1 = btCos(btScalar(i)*SIMD_RADS_PER_DEG) * radius1;
+        float x2 = btSin(btScalar(i)*SIMD_RADS_PER_DEG) * radius2;
+        float z2 = btCos(btScalar(i)*SIMD_RADS_PER_DEG) * radius2;
+
+        btVector3 addVector1 = xAxis * x1 + zAxis * z1;
+        btVector3 addVector2 = xAxis * x2 + zAxis * z2;
+        drawer->drawLine(position1 + addVector1, position2 + addVector2, color);
+    }
+}
+
+void ThreadSafeDynamicsWorld::debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color) {
+    btCollisionWorld::debugDrawObject(worldTransform, shape, color);
+    if (shape->getShapeType() == MULTI_SPHERE_SHAPE_PROXYTYPE) {
+        const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape);
+        for (int i = multiSphereShape->getSphereCount() - 1; i >= 0; i--) {
+            btTransform sphereTransform1, sphereTransform2;
+            sphereTransform1.setIdentity();
+            sphereTransform2.setIdentity();
+            int sphereIndex1 = i;
+            int sphereIndex2 = i > 0 ? i - 1 : multiSphereShape->getSphereCount() - 1;
+            sphereTransform1.setOrigin(multiSphereShape->getSpherePosition(sphereIndex1));
+            sphereTransform2.setOrigin(multiSphereShape->getSpherePosition(sphereIndex2));
+            sphereTransform1 = worldTransform * sphereTransform1;
+            sphereTransform2 = worldTransform * sphereTransform2;
+            getDebugDrawer()->drawSphere(multiSphereShape->getSphereRadius(sphereIndex1), sphereTransform1, color);
+            drawConnectedSpheres(getDebugDrawer(), multiSphereShape->getSphereRadius(sphereIndex1), multiSphereShape->getSphereRadius(sphereIndex2), sphereTransform1.getOrigin(), sphereTransform2.getOrigin(), color);
+        }
+    } else {
+        btCollisionWorld::debugDrawObject(worldTransform, shape, color);
+    }
+}
+

libraries/physics/src/ThreadSafeDynamicsWorld.h

@@ -53,10 +53,13 @@ public:
     const VectorOfMotionStates& getDeactivatedMotionStates() const { return _deactivatedStates; }
 
     void addChangedMotionState(ObjectMotionState* motionState) { _changedMotionStates.push_back(motionState); }
+    virtual void debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color) override;
 
 private:
     // call this instead of non-virtual btDiscreteDynamicsWorld::synchronizeSingleMotionState()
     void synchronizeMotionState(btRigidBody* body);
+    void drawConnectedSpheres(btIDebugDraw* drawer, btScalar radius1, btScalar radius2, const btVector3& position1, 
+                              const btVector3& position2, const btVector3& color);
 
     VectorOfMotionStates _changedMotionStates;
     VectorOfMotionStates _deactivatedStates;

libraries/shared/src/DebugDraw.cpp

@@ -74,3 +74,13 @@ void DebugDraw::clearRays() {
     Lock lock(_mapMutex);
     _rays.clear();
 }
+
+void DebugDraw::drawRays(const std::vector<std::pair<glm::vec3, glm::vec3>>& lines,
+    const glm::vec4& color, const glm::vec3& translation, const glm::quat& rotation) {
+    Lock lock(_mapMutex);
+    for (std::pair<glm::vec3, glm::vec3> line : lines) {
+        auto point1 = translation + rotation * line.first;
+        auto point2 = translation + rotation * line.second;
+        _rays.push_back(Ray(point1, point2, color));
+    }
+}

libraries/shared/src/DebugDraw.h

@@ -47,6 +47,16 @@ public:
      * @param {Vec4} color - color of line, each component should be in the zero to one range.  x = red, y = blue, z = green, w = alpha.
      */
     Q_INVOKABLE void drawRay(const glm::vec3& start, const glm::vec3& end, const glm::vec4& color);
+    
+    /**jsdoc
+    * Draws a line in world space, but it will only be visible for a single frame.
+    * @function DebugDraw.drawRay
+    * @param {Vec3} start - start position of line in world space.
+    * @param {Vec3} end - end position of line in world space.
+    * @param {Vec4} color - color of line, each component should be in the zero to one range.  x = red, y = blue, z = green, w = alpha.
+    */
+    Q_INVOKABLE void drawRays(const std::vector<std::pair<glm::vec3, glm::vec3>>& lines, const glm::vec4& color,
+                              const glm::vec3& translation = glm::vec3(0.0f, 0.0f, 0.0f), const glm::quat& rotation = glm::quat(1.0f, 0.0f, 0.0f, 0.0f));
 
     /**jsdoc
      * Adds a debug marker to the world. This marker will be drawn every frame until it is removed with DebugDraw.removeMarker.