Merge pull request #13833 from sabrina-shanman/stt_performance

Performance improvements for Collision Pick API

interface/src/raypick/CollisionPick.cpp

@@ -12,9 +12,33 @@
 
 #include <glm/gtx/transform.hpp>
 
+#include "PhysicsCollisionGroups.h"
 #include "ScriptEngineLogging.h"
 #include "UUIDHasher.h"
 
+PickResultPointer CollisionPickResult::compareAndProcessNewResult(const PickResultPointer& newRes) {
+    const std::shared_ptr<CollisionPickResult> newCollisionResult = std::static_pointer_cast<CollisionPickResult>(newRes);
+
+    if (entityIntersections.size()) {
+        entityIntersections.insert(entityIntersections.cend(), newCollisionResult->entityIntersections.begin(), newCollisionResult->entityIntersections.end());
+    } else {
+        entityIntersections = newCollisionResult->entityIntersections;
+    }
+
+    if (avatarIntersections.size()) {
+        avatarIntersections.insert(avatarIntersections.cend(), newCollisionResult->avatarIntersections.begin(), newCollisionResult->avatarIntersections.end());
+    } else {
+        avatarIntersections = newCollisionResult->avatarIntersections;
+    }
+
+    intersects = entityIntersections.size() || avatarIntersections.size();
+    if (newCollisionResult->loadState == LOAD_STATE_NOT_LOADED || loadState == LOAD_STATE_UNKNOWN) {
+        loadState = (LoadState)newCollisionResult->loadState;
+    }
+
+    return std::make_shared<CollisionPickResult>(*this);
+}
+
 void buildObjectIntersectionsMap(IntersectionType intersectionType, const std::vector<ContactTestResult>& objectIntersections, std::unordered_map<QUuid, QVariantMap>& intersections, std::unordered_map<QUuid, QVariantList>& collisionPointPairs) {
     for (auto& objectIntersection : objectIntersections) {
         auto at = intersections.find(objectIntersection.foundID);
@@ -308,20 +332,27 @@ CollisionRegion CollisionPick::getMathematicalPick() const {
     return _mathPick;
 }
 
-const std::vector<ContactTestResult> CollisionPick::filterIntersections(const std::vector<ContactTestResult>& intersections) const {
+void CollisionPick::filterIntersections(std::vector<ContactTestResult>& intersections) const {
-    std::vector<ContactTestResult> filteredIntersections;
-
     const QVector<QUuid>& ignoreItems = getIgnoreItems();
     const QVector<QUuid>& includeItems = getIncludeItems();
-    bool isWhitelist = includeItems.size();
+    bool isWhitelist = !includeItems.empty();
-    for (const auto& intersection : intersections) {
+
+    if (!isWhitelist && ignoreItems.empty()) {
+        return;
+    }
+
+    std::vector<ContactTestResult> filteredIntersections;
+
+    int n = (int)intersections.size();
+    for (int i = 0; i < n; i++) {
+        auto& intersection = intersections[i];
         const QUuid& id = intersection.foundID;
         if (!ignoreItems.contains(id) && (!isWhitelist || includeItems.contains(id))) {
             filteredIntersections.push_back(intersection);
         }
     }
 
-    return filteredIntersections;
+    intersections = filteredIntersections;
 }
 
 PickResultPointer CollisionPick::getEntityIntersection(const CollisionRegion& pick) {
@@ -330,7 +361,8 @@ PickResultPointer CollisionPick::getEntityIntersection(const CollisionRegion& pi
         return std::make_shared<CollisionPickResult>(pick.toVariantMap(), CollisionPickResult::LOAD_STATE_NOT_LOADED, std::vector<ContactTestResult>(), std::vector<ContactTestResult>());
     }
     
-    const auto& entityIntersections = filterIntersections(_physicsEngine->getCollidingInRegion(MOTIONSTATE_TYPE_ENTITY, *pick.shapeInfo, pick.transform));
+    auto entityIntersections = _physicsEngine->contactTest(USER_COLLISION_MASK_ENTITIES, *pick.shapeInfo, pick.transform);
+    filterIntersections(entityIntersections);
     return std::make_shared<CollisionPickResult>(pick, CollisionPickResult::LOAD_STATE_LOADED, entityIntersections, std::vector<ContactTestResult>());
 }
 
@@ -343,8 +375,9 @@ PickResultPointer CollisionPick::getAvatarIntersection(const CollisionRegion& pi
         // Cannot compute result
         return std::make_shared<CollisionPickResult>(pick.toVariantMap(), CollisionPickResult::LOAD_STATE_NOT_LOADED, std::vector<ContactTestResult>(), std::vector<ContactTestResult>());
     }
-
+    
-    const auto& avatarIntersections = filterIntersections(_physicsEngine->getCollidingInRegion(MOTIONSTATE_TYPE_AVATAR, *pick.shapeInfo, pick.transform));
+    auto avatarIntersections = _physicsEngine->contactTest(USER_COLLISION_MASK_AVATARS, *pick.shapeInfo, pick.transform);
+    filterIntersections(avatarIntersections);
     return std::make_shared<CollisionPickResult>(pick, CollisionPickResult::LOAD_STATE_LOADED, std::vector<ContactTestResult>(), avatarIntersections);
 }
 

interface/src/raypick/CollisionPick.h

@@ -49,23 +49,7 @@ public:
     bool doesIntersect() const override { return intersects; }
     bool checkOrFilterAgainstMaxDistance(float maxDistance) override { return true; }
 
-    PickResultPointer compareAndProcessNewResult(const PickResultPointer& newRes) override {
+    PickResultPointer compareAndProcessNewResult(const PickResultPointer& newRes) override;
-        const std::shared_ptr<CollisionPickResult> newCollisionResult = std::static_pointer_cast<CollisionPickResult>(newRes);
-
-        for (ContactTestResult& entityIntersection : newCollisionResult->entityIntersections) {
-            entityIntersections.push_back(entityIntersection);
-        }
-        for (ContactTestResult& avatarIntersection : newCollisionResult->avatarIntersections) {
-            avatarIntersections.push_back(avatarIntersection);
-        }
-
-        intersects = entityIntersections.size() || avatarIntersections.size();
-        if (newCollisionResult->loadState == LOAD_STATE_NOT_LOADED || loadState == LOAD_STATE_UNKNOWN) {
-            loadState = (LoadState)newCollisionResult->loadState;        
-        }
-
-        return std::make_shared<CollisionPickResult>(*this);
-    }
 };
 
 class CollisionPick : public Pick<CollisionRegion> {
@@ -92,7 +76,7 @@ protected:
     // Returns true if pick.shapeInfo is valid. Otherwise, attempts to get the shapeInfo ready for use.
     bool isShapeInfoReady();
     void computeShapeInfo(CollisionRegion& pick, ShapeInfo& shapeInfo, QSharedPointer<GeometryResource> resource);
-    const std::vector<ContactTestResult> filterIntersections(const std::vector<ContactTestResult>& intersections) const;
+    void filterIntersections(std::vector<ContactTestResult>& intersections) const;
 
     CollisionRegion _mathPick;
     PhysicsEnginePointer _physicsEngine;

libraries/physics/src/PhysicsEngine.cpp

@@ -843,9 +843,8 @@ void PhysicsEngine::setShowBulletConstraintLimits(bool value) {
 }
 
 struct AllContactsCallback : public btCollisionWorld::ContactResultCallback {
-    AllContactsCallback(MotionStateType desiredObjectType, const ShapeInfo& shapeInfo, const Transform& transform, btCollisionObject* myAvatarCollisionObject) :
+    AllContactsCallback(int32_t mask, int32_t group, const ShapeInfo& shapeInfo, const Transform& transform, btCollisionObject* myAvatarCollisionObject) :
         btCollisionWorld::ContactResultCallback(),
-        desiredObjectType(desiredObjectType),
         collisionObject(),
         contacts(),
         myAvatarCollisionObject(myAvatarCollisionObject) {
@@ -858,21 +857,19 @@ struct AllContactsCallback : public btCollisionWorld::ContactResultCallback {
         bulletTransform.setRotation(glmToBullet(transform.getRotation()));
 
         collisionObject.setWorldTransform(bulletTransform);
+
+        m_collisionFilterMask = mask;
+        m_collisionFilterGroup = group;
     }
 
     ~AllContactsCallback() {
         ObjectMotionState::getShapeManager()->releaseShape(collisionObject.getCollisionShape());
     }
 
-    MotionStateType desiredObjectType;
     btCollisionObject collisionObject;
     std::vector<ContactTestResult> contacts;
     btCollisionObject* myAvatarCollisionObject;
 
-    bool needsCollision(btBroadphaseProxy* proxy) const override {
-        return true;
-    }
-
     btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0, int partId0, int index0, const btCollisionObjectWrapper* colObj1, int partId1, int index1) override {
         const btCollisionObject* otherBody;
         btVector3 penetrationPoint;
@@ -888,7 +885,7 @@ struct AllContactsCallback : public btCollisionWorld::ContactResultCallback {
         }
 
         // TODO: Give MyAvatar a motion state so we don't have to do this
-        if (desiredObjectType == MOTIONSTATE_TYPE_AVATAR && myAvatarCollisionObject && myAvatarCollisionObject == otherBody) {
+        if ((m_collisionFilterMask & BULLET_COLLISION_GROUP_MY_AVATAR) && myAvatarCollisionObject && myAvatarCollisionObject == otherBody) {
             contacts.emplace_back(Physics::getSessionUUID(), bulletToGLM(penetrationPoint), bulletToGLM(otherPenetrationPoint));
             return 0;
         }
@@ -900,7 +897,7 @@ struct AllContactsCallback : public btCollisionWorld::ContactResultCallback {
 
         const btMotionState* motionStateCandidate = collisionCandidate->getMotionState();
         const ObjectMotionState* candidate = dynamic_cast<const ObjectMotionState*>(motionStateCandidate);
-        if (!candidate || candidate->getType() != desiredObjectType) {
+        if (!candidate) {
             return 0;
         }
 
@@ -916,14 +913,14 @@ protected:
     }
 };
 
-const std::vector<ContactTestResult> PhysicsEngine::getCollidingInRegion(MotionStateType desiredObjectType, const ShapeInfo& regionShapeInfo, const Transform& regionTransform) const {
+std::vector<ContactTestResult> PhysicsEngine::contactTest(uint16_t mask, const ShapeInfo& regionShapeInfo, const Transform& regionTransform, uint16_t group) const {
     // TODO: Give MyAvatar a motion state so we don't have to do this
     btCollisionObject* myAvatarCollisionObject = nullptr;
-    if (desiredObjectType == MOTIONSTATE_TYPE_AVATAR && _myAvatarController) {
+    if ((mask & USER_COLLISION_GROUP_MY_AVATAR) && _myAvatarController) {
         myAvatarCollisionObject = _myAvatarController->getCollisionObject();
     }
 
-    auto contactCallback = AllContactsCallback(desiredObjectType, regionShapeInfo, regionTransform, myAvatarCollisionObject);
+    auto contactCallback = AllContactsCallback((int32_t)mask, (int32_t)group, regionShapeInfo, regionTransform, myAvatarCollisionObject);
     _dynamicsWorld->contactTest(&contactCallback.collisionObject, contactCallback);
 
     return contactCallback.contacts;

libraries/physics/src/PhysicsEngine.h

@@ -125,8 +125,9 @@ public:
     void setShowBulletConstraints(bool value);
     void setShowBulletConstraintLimits(bool value);
 
-    // Function for getting colliding ObjectMotionStates in the world of specified type
+    // Function for getting colliding objects in the world of specified type
-    const std::vector<ContactTestResult> getCollidingInRegion(MotionStateType desiredObjectType, const ShapeInfo& regionShapeInfo, const Transform& regionTransform) const;
+    // See PhysicsCollisionGroups.h for mask flags.
+    std::vector<ContactTestResult> contactTest(uint16_t mask, const ShapeInfo& regionShapeInfo, const Transform& regionTransform, uint16_t group = USER_COLLISION_GROUP_DYNAMIC) const;
 
 private:
     QList<EntityDynamicPointer> removeDynamicsForBody(btRigidBody* body);

libraries/shared/src/PhysicsCollisionGroups.h

@@ -104,6 +104,7 @@ const uint16_t ENTITY_COLLISION_MASK_DEFAULT =
     USER_COLLISION_GROUP_OTHER_AVATAR;
 
 const uint16_t USER_COLLISION_MASK_AVATARS = USER_COLLISION_GROUP_MY_AVATAR | USER_COLLISION_GROUP_OTHER_AVATAR;
+const uint16_t USER_COLLISION_MASK_ENTITIES = USER_COLLISION_GROUP_STATIC | USER_COLLISION_GROUP_DYNAMIC | USER_COLLISION_GROUP_KINEMATIC;
 
 const int32_t NUM_USER_COLLISION_GROUPS = 5;