Suggested changes

interface/src/avatar/AvatarManager.cpp

@@ -411,11 +411,8 @@ void AvatarManager::buildPhysicsTransaction(PhysicsEngine::Transaction& transact
                 
                 auto& detailedMotionStates = avatar->getDetailedMotionStates();
                 for (auto& mState : detailedMotionStates) {
-                    if (mState) {
+                    transaction.objectsToRemove.push_back(mState);
-                        transaction.objectsToRemove.push_back(mState);
-                    }
                 }
-                qDebug() << "Removing " << detailedMotionStates.size() << " detailed motion states from " << avatar->getSessionUUID();
                 avatar->resetDetailedMotionStates();
                 
             } else {
@@ -430,15 +427,12 @@ void AvatarManager::buildPhysicsTransaction(PhysicsEngine::Transaction& transact
                 } else {
                     failedShapeBuilds.insert(avatar);
                 }
-
                 if (avatar->getDetailedMotionStates().size() == 0) {
                     avatar->createDetailedMotionStates(avatar);
                     for (auto dMotionState : avatar->getDetailedMotionStates()) {
                         transaction.objectsToAdd.push_back(dMotionState);
                     }
                 }
-                
-                qDebug() << "Adding " << avatar->getDetailedMotionStates().size() << " detailed motion states from " << avatar->getSessionUUID();
             }
         } else if (isInPhysics) {
             transaction.objectsToChange.push_back(avatar->_motionState);
@@ -642,19 +636,17 @@ AvatarSharedPointer AvatarManager::getAvatarBySessionID(const QUuid& sessionID)
 RayToAvatarIntersectionResult AvatarManager::findRayIntersection(const PickRay& ray,
                                                                  const QScriptValue& avatarIdsToInclude,
                                                                  const QScriptValue& avatarIdsToDiscard,
-                                                                 const QScriptValue& jointIndicesToFilter,
+                                                                 const QStringList& jointIndicesToFilter,
                                                                  bool pickAgainstMesh) {
     QVector<EntityItemID> avatarsToInclude = qVectorEntityItemIDFromScriptValue(avatarIdsToInclude);
     QVector<EntityItemID> avatarsToDiscard = qVectorEntityItemIDFromScriptValue(avatarIdsToDiscard);
-    QVector<uint> jointsToFilter;
+    return findRayIntersectionVector(ray, avatarsToInclude, avatarsToDiscard, jointIndicesToFilter, pickAgainstMesh);
-    qVectorIntFromScriptValue(jointIndicesToFilter, jointsToFilter);
-    return findRayIntersectionVector(ray, avatarsToInclude, avatarsToDiscard, jointsToFilter, pickAgainstMesh);
 }
 
 RayToAvatarIntersectionResult AvatarManager::findRayIntersectionVector(const PickRay& ray,
                                                                        const QVector<EntityItemID>& avatarsToInclude,
                                                                        const QVector<EntityItemID>& avatarsToDiscard,
-                                                                       const QVector<uint>& jointIndicesToFilter,
+                                                                       const QStringList& jointIndicesToFilter,
                                                                        bool pickAgainstMesh) {
     RayToAvatarIntersectionResult result;
     if (QThread::currentThread() != thread()) {
@@ -663,14 +655,10 @@ RayToAvatarIntersectionResult AvatarManager::findRayIntersectionVector(const Pic
                                   Q_ARG(const PickRay&, ray),
                                   Q_ARG(const QVector<EntityItemID>&, avatarsToInclude),
                                   Q_ARG(const QVector<EntityItemID>&, avatarsToDiscard),
-                                  Q_ARG(const QVector<uint>&, jointIndicesToFilter),
+                                  Q_ARG(const QStringList&, jointIndicesToFilter),
                                   Q_ARG(bool, pickAgainstMesh));
         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;
@@ -681,13 +669,18 @@ RayToAvatarIntersectionResult AvatarManager::findRayIntersectionVector(const Pic
     glm::vec3 transformedRayDirection;
     
     if (physicsResults.size() > 0) {
+        glm::vec3 rayDirectionInv = { ray.direction.x != 0 ? 1.0f / ray.direction.x : INFINITY,
+            ray.direction.y != 0.0f ? 1.0f / ray.direction.y : INFINITY,
+            ray.direction.z != 0.0f ? 1.0f / ray.direction.z : INFINITY };
+
         MyCharacterController::RayAvatarResult rayAvatarResult;
         AvatarPointer avatar = nullptr;
         for (auto &hit : physicsResults) {
             auto avatarID = hit._intersectWithAvatar;
-            bool skipThisAvatar = ((avatarsToInclude.size() > 0 && !avatarsToInclude.contains(avatarID)) ||
+            bool avatarIsIncluded = avatarsToInclude.contains(avatarID);
-                                  (avatarsToDiscard.size() > 0 && avatarsToDiscard.contains(avatarID))) && jointIndicesToFilter.size() == 0;
+            bool avatarIsDiscarded = avatarsToDiscard.contains(avatarID);
-            if (skipThisAvatar) {
+            if (jointIndicesToFilter.size() == 0 && ((avatarsToInclude.size() > 0 && !avatarIsIncluded) ||
+                                                     (avatarsToDiscard.size() > 0 && avatarIsDiscarded))) {
                 continue;
             }
             if (!(_myAvatar->getSessionUUID() == avatarID)) {
@@ -701,16 +694,16 @@ RayToAvatarIntersectionResult AvatarManager::findRayIntersectionVector(const Pic
             }
             QVector<int> jointsToDiscard;
             if (avatar && jointIndicesToFilter.size() > 0) {
-                int jointCount = avatar->getJointCount();
+                auto names = avatar->getJointNames();
-                if (avatarsToInclude.size() > 0 && avatarsToInclude.contains(avatarID)) {
+                if (avatarIsIncluded) {
-                    for (int i = 0; i < jointCount; i++) {
+                    for (int i = 0; i < names.size(); i++) {
-                        if (!jointIndicesToFilter.contains(i)) {
+                        if (!jointIndicesToFilter.contains(names[i])) {
                             jointsToDiscard.push_back(i);
                         }
                     }
-                } else if (avatarsToDiscard.size() > 0 && avatarsToDiscard.contains(avatarID)) {
+                } else if (avatarIsDiscarded) {
-                    for (int i = 0; i < jointCount; i++) {
+                    for (int i = 0; i < names.size(); i++) {
-                        if (jointIndicesToFilter.contains(i)) {
+                        if (jointIndicesToFilter.contains(names[i])) {
                             jointsToDiscard.push_back(i);
                         }
                     }
@@ -719,7 +712,6 @@ RayToAvatarIntersectionResult AvatarManager::findRayIntersectionVector(const Pic
             if (!hit._isBound) {
                 if (!jointsToDiscard.contains(hit._intersectWithJoint)) {
                     rayAvatarResult = hit;
-                    break;
                 }
             } else if (avatar) {
                 auto &multiSpheres = avatar->getMultiSphereShapes();
@@ -753,13 +745,9 @@ RayToAvatarIntersectionResult AvatarManager::findRayIntersectionVector(const Pic
                             });
                         } 
                         rayAvatarResult = boxHits[0];
-                        break;
                     }
                 }
             }
-        }
-
-        if (rayAvatarResult._intersect) {
             if (pickAgainstMesh) {
                 glm::vec3 localRayOrigin = avatar->worldToJointPoint(ray.origin, rayAvatarResult._intersectWithJoint);
                 glm::vec3 localRayPoint = avatar->worldToJointPoint(ray.origin + ray.direction, rayAvatarResult._intersectWithJoint);
@@ -769,7 +757,7 @@ RayToAvatarIntersectionResult AvatarManager::findRayIntersectionVector(const Pic
 
                 auto jointOrientation = avatarOrientation * avatar->getAbsoluteDefaultJointRotationInObjectFrame(rayAvatarResult._intersectWithJoint);
                 auto jointPosition = avatarPosition + (avatarOrientation * avatar->getAbsoluteDefaultJointTranslationInObjectFrame(rayAvatarResult._intersectWithJoint));
-                
+
                 auto defaultFrameRayOrigin = jointPosition + jointOrientation * localRayOrigin;
                 auto defaultFrameRayPoint = jointPosition + jointOrientation * localRayPoint;
                 auto defaultFrameRayDirection = defaultFrameRayPoint - defaultFrameRayOrigin;
@@ -780,9 +768,14 @@ RayToAvatarIntersectionResult AvatarManager::findRayIntersectionVector(const Pic
                     rayAvatarResult._intersectionPoint = ray.origin + newDistance * glm::normalize(ray.direction);
                     rayAvatarResult._intersectionNormal = surfaceNormal;
                     extraInfo["worldIntersectionPoint"] = vec3toVariant(rayAvatarResult._intersectionPoint);
+                    break;
                 }
+            } else {
+                break;
             }
+        }
 
+        if (rayAvatarResult._intersect) {
             result.intersects = true;
             result.avatarID = rayAvatarResult._intersectWithAvatar;
             result.distance = rayAvatarResult._distance;

interface/src/avatar/AvatarManager.h

@@ -138,26 +138,26 @@ public:
      * @param {PickRay} ray
      * @param {Uuid[]} [avatarsToInclude=[]]
      * @param {Uuid[]} [avatarsToDiscard=[]]
-     * @param {uint[]} [jointIndicesToFilter=[] - If included/discarded avatars are provided only this joints corresponding to those avatars would be included/discarded. ]
+     * @param {string[]} [jointIndicesToFilter=[] - If included/discarded avatars are provided only this joints corresponding to those avatars would be included/discarded. ]
      * @returns {RayToAvatarIntersectionResult}
      */
     Q_INVOKABLE RayToAvatarIntersectionResult findRayIntersection(const PickRay& ray,
                                                                   const QScriptValue& avatarIdsToInclude = QScriptValue(),
                                                                   const QScriptValue& avatarIdsToDiscard = QScriptValue(),
-                                                                  const QScriptValue& jointIndicesToFilter = QScriptValue(),
+                                                                  const QStringList& jointIndicesToFilter = QStringList(),
-                                                                  bool pickAgainstMesh = false);
+                                                                  bool pickAgainstMesh = true);
     /**jsdoc
      * @function AvatarManager.findRayIntersectionVector
      * @param {PickRay} ray
      * @param {Uuid[]} avatarsToInclude
      * @param {Uuid[]} avatarsToDiscard
-     * @param {uint[]} [jointIndicesToFilter=[] - If included/discarded avatars are provided only this joints corresponding to those avatars would be included/discarded. ]
+     * @param {string[]} [jointIndicesToFilter=[] - If included/discarded avatars are provided only this joints corresponding to those avatars would be included/discarded. ]
      * @returns {RayToAvatarIntersectionResult}
      */
     Q_INVOKABLE RayToAvatarIntersectionResult findRayIntersectionVector(const PickRay& ray,
                                                                         const QVector<EntityItemID>& avatarsToInclude,
                                                                         const QVector<EntityItemID>& avatarsToDiscard,
-                                                                        const QVector<uint>& jointIndicesToFilter, 
+                                                                        const QStringList& jointIndicesToFilter,
                                                                         bool pickAgainstMesh);
 
     /**jsdoc

interface/src/avatar/MyCharacterController.cpp

@@ -47,7 +47,6 @@ 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();
@@ -378,6 +377,12 @@ DetailedMotionState* MyCharacterController::createDetailedMotionStateForJoint(in
     return nullptr;
 }
 
+void MyCharacterController::clearDetailedMotionStates() {
+    _pendingFlags |= PENDING_FLAG_REMOVE_DETAILED_FROM_SIMULATION; 
+    // We make sure we don't add them again
+    _pendingFlags &= ~PENDING_FLAG_ADD_DETAILED_TO_SIMULATION;
+}
+
 void MyCharacterController::resetDetailedMotionStates() {
     for (size_t i = 0; i < _detailedMotionStates.size(); i++) {
         _detailedMotionStates[i] = nullptr;
@@ -455,6 +460,10 @@ std::vector<MyCharacterController::RayAvatarResult> MyCharacterController::rayTe
                     result._distance = length * rayCallback.m_hitFractions[i];
                     result._intersectWithJoint = detailedMotionState->getJointIndex();
                     result._isBound = detailedMotionState->getIsBound(result._boundJoints);
+                    btVector3 center;
+                    btScalar radius;
+                    detailedMotionState->getShape()->getBoundingSphere(center, radius);
+                    result._maxDistance = (float)radius;
                     foundAvatars.push_back(result);
                 }
             }

interface/src/avatar/MyCharacterController.h

@@ -47,7 +47,7 @@ public:
     btCollisionShape* createDetailedCollisionShapeForJoint(int jointIndex);
     DetailedMotionState* createDetailedMotionStateForJoint(int jointIndex);
     std::vector<DetailedMotionState*>& getDetailedMotionStates() { return _detailedMotionStates; }
-    void clearDetailedMotionStates() { _pendingFlags |= PENDING_FLAG_REMOVE_DETAILED_FROM_SIMULATION; }
+    void clearDetailedMotionStates();
     void resetDetailedMotionStates();
 
     void buildPhysicsTransaction(PhysicsEngine::Transaction& transaction);
@@ -60,6 +60,8 @@ public:
         QUuid _intersectWithAvatar;
         int _intersectWithJoint { -1 };
         float _distance { 0.0f };
+        float _maxDistance { 0.0f };
+        QVariantMap _extraInfo;
         glm::vec3 _intersectionPoint;
         glm::vec3 _intersectionNormal;
         std::vector<int> _boundJoints;

interface/src/avatar/OtherAvatar.cpp

@@ -210,7 +210,7 @@ void OtherAvatar::setWorkloadRegion(uint8_t region) {
     } else {
         printRegion = "invalid";
     }
-    qDebug() << "Setting workload region to " << printRegion;
+    qCDebug(avatars) << "Setting workload region to " << printRegion;
     computeShapeLOD();
 }
 
@@ -235,7 +235,7 @@ void OtherAvatar::computeShapeLOD() {
     if (newLOD != _bodyLOD) {
         _bodyLOD = newLOD;
         if (isInPhysicsSimulation()) {
-            qDebug() << "Changing to body LOD " << newLOD;
+            qCDebug(avatars) << "Changing to body LOD " << newLOD;
             _needsReinsertion = true;
         }
     }
@@ -280,6 +280,7 @@ void OtherAvatar::updateCollisionGroup(bool myAvatarCollide) {
 }
 
 void OtherAvatar::createDetailedMotionStates(const std::shared_ptr<OtherAvatar>& avatar) {
+    assert(detailedMotionStates.empty());
     auto& detailedMotionStates = getDetailedMotionStates();
     if (_bodyLOD == BodyLOD::Sphere) {
         auto dMotionState = createMotionState(avatar, -1);

interface/src/raypick/RayPick.cpp

@@ -56,7 +56,7 @@ PickResultPointer RayPick::getOverlayIntersection(const PickRay& pick) {
 }
 
 PickResultPointer RayPick::getAvatarIntersection(const PickRay& pick) {
-    RayToAvatarIntersectionResult avatarRes = DependencyManager::get<AvatarManager>()->findRayIntersectionVector(pick, getIncludeItemsAs<EntityItemID>(), getIgnoreItemsAs<EntityItemID>(), QVector<uint>(), false);
+    RayToAvatarIntersectionResult avatarRes = DependencyManager::get<AvatarManager>()->findRayIntersectionVector(pick, getIncludeItemsAs<EntityItemID>(), getIgnoreItemsAs<EntityItemID>(), QStringList(), true);
     if (avatarRes.intersects) {
         return std::make_shared<RayPickResult>(IntersectionType::AVATAR, avatarRes.avatarID, avatarRes.distance, avatarRes.intersection, pick, avatarRes.surfaceNormal, avatarRes.extraInfo);
     } else {

libraries/physics/src/MultiSphereShape.cpp

@@ -25,7 +25,7 @@ void SphereRegion::dump(std::vector<std::pair<glm::vec3, glm::vec3>>& outLines)
 
 void SphereRegion::insertUnique(const glm::vec3& point, std::vector<glm::vec3>& pointSet) {
     auto hit = std::find_if(pointSet.begin(), pointSet.end(), [point](const glm::vec3& pointFromSet) -> bool {
-        return (pointFromSet == point);
+        return (glm::length(pointFromSet-point) < FLT_EPSILON);
     });
     if (hit == pointSet.end()) {
         pointSet.push_back(point);
@@ -107,7 +107,6 @@ CollisionShapeExtractionMode MultiSphereShape::getExtractionModeByName(const QSt
     }
     else if (isSim || isFlow || isEye || isToe) {
         mode = CollisionShapeExtractionMode::None;
-        //qDebug() << "Trying to add " << (int)positions.size() << " spheres for " << jointName << " length: " << maxLength;
     }
     return mode;
 }
@@ -116,9 +115,9 @@ void MultiSphereShape::filterUniquePoints(const std::vector<btVector3>& kdop, st
     for (size_t j = 0; j < kdop.size(); j++) {
         btVector3 btPoint = kdop[j];
         auto hit = std::find_if(uniquePoints.begin(), uniquePoints.end(), [btPoint](const glm::vec3& point) -> bool {
-            return (btPoint.getX() == point.x
+            return (glm::length(btPoint.getX() - point.x) < FLT_EPSILON
-                && btPoint.getY() == point.y
+                && glm::length(btPoint.getY() - point.y) < FLT_EPSILON
-                && btPoint.getZ() == point.z);
+                && glm::length(btPoint.getZ() - point.z) < FLT_EPSILON);
         });
         if (hit == uniquePoints.end()) {
             uniquePoints.push_back(bulletToGLM(btPoint));
@@ -287,9 +286,11 @@ void MultiSphereShape::spheresFromAxes(const std::vector<glm::vec3>& points, con
                 maxRadius = radius > maxRadius ? radius : maxRadius;
             }
         }
-        averageRadius /= (int)points.size();
+        if (points.size() > 0) {
-        maxAverageRadius = averageRadius > maxAverageRadius ? averageRadius : maxAverageRadius;
+            averageRadius /= (int)points.size();
-        minAverageRadius = averageRadius < minAverageRadius ? averageRadius : minAverageRadius;
+        }
+        maxAverageRadius = glm::max(averageRadius, maxAverageRadius);
+        minAverageRadius = glm::min(averageRadius, minAverageRadius);
         spheres[j]._radius = averageRadius;
     }
     float radiusRatio = maxRadius / maxAverageRadius;
@@ -331,17 +332,17 @@ void MultiSphereShape::connectSpheres(int index1, int index2, bool onlyEdges) {
     auto axis = sphere1._position - sphere2._position;
 
     float angleOffset = glm::asin((sphere1._radius - sphere2._radius) / distance);
-    float percent1 = ((0.5f * PI) + angleOffset) / PI;
+    float ratio1 = ((0.5f * PI) + angleOffset) / PI;
-    float percent2 = ((0.5f * PI) - angleOffset) / PI;
+    float ratio2 = ((0.5f * PI) - angleOffset) / PI;
 
     std::vector<glm::vec3> edge1, edge2;
     if (onlyEdges) {
         std::vector<std::pair<glm::vec3, glm::vec3>> debugLines;
-        calculateSphereLines(debugLines, sphere1._position, sphere1._radius, DEFAULT_SPHERE_SUBDIVISIONS, glm::normalize(axis), percent1, &edge1);
+        calculateSphereLines(debugLines, sphere1._position, sphere1._radius, DEFAULT_SPHERE_SUBDIVISIONS, glm::normalize(axis), ratio1, &edge1);
-        calculateSphereLines(debugLines, sphere2._position, sphere2._radius, DEFAULT_SPHERE_SUBDIVISIONS, glm::normalize(-axis), percent2, &edge2);
+        calculateSphereLines(debugLines, sphere2._position, sphere2._radius, DEFAULT_SPHERE_SUBDIVISIONS, glm::normalize(-axis), ratio2, &edge2);
     } else {
-        calculateSphereLines(_debugLines, sphere1._position, sphere1._radius, DEFAULT_SPHERE_SUBDIVISIONS, glm::normalize(axis), percent1, &edge1);
+        calculateSphereLines(_debugLines, sphere1._position, sphere1._radius, DEFAULT_SPHERE_SUBDIVISIONS, glm::normalize(axis), ratio1, &edge1);
-        calculateSphereLines(_debugLines, sphere2._position, sphere2._radius, DEFAULT_SPHERE_SUBDIVISIONS, glm::normalize(-axis), percent2, &edge2);
+        calculateSphereLines(_debugLines, sphere2._position, sphere2._radius, DEFAULT_SPHERE_SUBDIVISIONS, glm::normalize(-axis), ratio2, &edge2);
     }
     connectEdges(_debugLines, edge1, edge2);
 }

libraries/physics/src/ShapeFactory.cpp

@@ -289,8 +289,8 @@ const btCollisionShape* ShapeFactory::createShapeFromInfo(const ShapeInfo& info)
             std::vector<float> radiuses;
             auto sphereCollection = info.getSphereCollection();
             for (auto &sphereData : sphereCollection) {
-                positions.push_back(glmToBullet(sphereData.first));
+                positions.push_back(glmToBullet(glm::vec3(sphereData)));
-                radiuses.push_back(sphereData.second);
+                radiuses.push_back(sphereData.w);
             }
             shape = new btMultiSphereShape(positions.data(), radiuses.data(), (int)positions.size());
         }

libraries/shared/src/ShapeInfo.cpp

@@ -276,8 +276,8 @@ const HashKey& ShapeInfo::getHash() const {
         _hashKey.hashUint64((uint64_t)_type);
         if (_type == SHAPE_TYPE_MULTISPHERE) {
             for (auto &sphereData : _sphereCollection) {
-                _hashKey.hashVec3(sphereData.first);
+                _hashKey.hashVec3(glm::vec3(sphereData));
-                _hashKey.hashFloat(sphereData.second);
+                _hashKey.hashFloat(sphereData.w);
             }
         } else if (_type != SHAPE_TYPE_SIMPLE_HULL) {
             _hashKey.hashVec3(_halfExtents);

libraries/shared/src/ShapeInfo.h

@@ -58,7 +58,7 @@ public:
     using PointList = QVector<glm::vec3>;
     using PointCollection = QVector<PointList>;
     using TriangleIndices = QVector<int32_t>;
-    using SphereData = QPair<glm::vec3, float>;
+    using SphereData = glm::vec4;
     using SphereCollection = QVector<SphereData>;
 
     static QString getNameForShapeType(ShapeType type);