Physics: Add support for merging collision groups.

When merging assets from multiple sources, it's desirable in some
scenarios that we wish to merge physics collision groups. This adds
support for optional user-provided identifiers which allow such
merging. We also add an option to invert the selection of filtered
groups, which can be used to create colliders which opt-in to
collisions, rather than the default behaviour of having to opt-out.

Additionally, added a utility function which will calculate a table
indicating if two groups collide as well as tests for said utility. This
utility can be used in authoring use-cases to provide feedback for
content creators as well as providing a reference which can be used to
validate an implementation.

pxr/usd/usdPhysics/CMakeLists.txt

@@ -92,6 +92,7 @@ pxr_library(usdPhysics
 pxr_test_scripts(
     testenv/testUsdPhysicsMetrics.py
     testenv/testUsdPhysicsRigidBodyAPI.py
+    testenv/testUsdPhysicsCollisionGroupAPI.py
 )
 
 pxr_register_test(testUsdPhysicsMetrics
@@ -106,3 +107,8 @@ pxr_register_test(testUsdPhysicsRigidBodyAPI
     EXPECTED_RETURN_CODE 0
 )
 
+pxr_register_test(testUsdPhysicsCollisionGroupAPI
+    PYTHON
+    COMMAND "${CMAKE_INSTALL_PREFIX}/tests/testUsdPhysicsCollisionGroupAPI"
+    EXPECTED_RETURN_CODE 0
+)

pxr/usd/usdPhysics/collisionGroup.cpp

@@ -103,6 +103,40 @@ UsdPhysicsCollisionGroup::_GetTfType() const
     return _GetStaticTfType();
 }
 
+UsdAttribute
+UsdPhysicsCollisionGroup::GetMergeGroupNameAttr() const
+{
+    return GetPrim().GetAttribute(UsdPhysicsTokens->physicsMergeGroup);
+}
+
+UsdAttribute
+UsdPhysicsCollisionGroup::CreateMergeGroupNameAttr(VtValue const &defaultValue, bool writeSparsely) const
+{
+    return UsdSchemaBase::_CreateAttr(UsdPhysicsTokens->physicsMergeGroup,
+                       SdfValueTypeNames->String,
+                       /* custom = */ false,
+                       SdfVariabilityVarying,
+                       defaultValue,
+                       writeSparsely);
+}
+
+UsdAttribute
+UsdPhysicsCollisionGroup::GetInvertFilteredGroupsAttr() const
+{
+    return GetPrim().GetAttribute(UsdPhysicsTokens->physicsInvertFilteredGroups);
+}
+
+UsdAttribute
+UsdPhysicsCollisionGroup::CreateInvertFilteredGroupsAttr(VtValue const &defaultValue, bool writeSparsely) const
+{
+    return UsdSchemaBase::_CreateAttr(UsdPhysicsTokens->physicsInvertFilteredGroups,
+                       SdfValueTypeNames->Bool,
+                       /* custom = */ false,
+                       SdfVariabilityVarying,
+                       defaultValue,
+                       writeSparsely);
+}
+
 UsdRelationship
 UsdPhysicsCollisionGroup::GetFilteredGroupsRel() const
 {
@@ -116,13 +150,30 @@ UsdPhysicsCollisionGroup::CreateFilteredGroupsRel() const
                        /* custom = */ false);
 }
 
+namespace {
+static inline TfTokenVector
+_ConcatenateAttributeNames(const TfTokenVector& left,const TfTokenVector& right)
+{
+    TfTokenVector result;
+    result.reserve(left.size() + right.size());
+    result.insert(result.end(), left.begin(), left.end());
+    result.insert(result.end(), right.begin(), right.end());
+    return result;
+}
+}
+
 /*static*/
 const TfTokenVector&
 UsdPhysicsCollisionGroup::GetSchemaAttributeNames(bool includeInherited)
 {
-    static TfTokenVector localNames;
+    static TfTokenVector localNames = {
+        UsdPhysicsTokens->physicsMergeGroup,
+        UsdPhysicsTokens->physicsInvertFilteredGroups,
+    };
     static TfTokenVector allNames =
-        UsdTyped::GetSchemaAttributeNames(true);
+        _ConcatenateAttributeNames(
+            UsdTyped::GetSchemaAttributeNames(true),
+            localNames);
 
     if (includeInherited)
         return allNames;
@@ -141,13 +192,182 @@ PXR_NAMESPACE_CLOSE_SCOPE
 // ===================================================================== //
 // --(BEGIN CUSTOM CODE)--
 
-PXR_NAMESPACE_OPEN_SCOPE
+#include "pxr/usd/usd/primRange.h"
+#include <algorithm>
 
+PXR_NAMESPACE_OPEN_SCOPE
 
 UsdCollectionAPI
 UsdPhysicsCollisionGroup::GetCollidersCollectionAPI() const
 {
     return UsdCollectionAPI(GetPrim(), UsdPhysicsTokens->colliders);
 }
 
+const SdfPathVector&
+UsdPhysicsCollisionGroup::CollisionGroupTable::GetCollisionGroups() const
+{
+    return groups;
+}
+
+bool
+UsdPhysicsCollisionGroup::CollisionGroupTable::IsCollisionEnabled(int idxA, int idxB) const
+{
+    if (idxA >= 0 && idxB >= 0 && idxA < groups.size() && idxB < groups.size())
+    {
+        int minGroup = std::min(idxA, idxB);
+        int maxGroup = std::max(idxA, idxB);
+        int numSkippedEntries = (minGroup * minGroup + minGroup) / 2;
+        return enabled[minGroup * groups.size() - numSkippedEntries + maxGroup];
+    }
+
+    // If the groups aren't in the table or we've been passed invalid groups,
+    // return true, as groups will collide by default.
+    return true;
+}
+
+bool
+UsdPhysicsCollisionGroup::CollisionGroupTable::IsCollisionEnabled(const SdfPath& primA, const SdfPath& primB) const
+{
+    auto a = std::find(groups.begin(), groups.end(), primA);
+    auto b = std::find(groups.begin(), groups.end(), primB);
+    return IsCollisionEnabled(std::distance(groups.begin(), a), std::distance(groups.begin(), b));
+}
+
+
+UsdPhysicsCollisionGroup::CollisionGroupTable
+UsdPhysicsCollisionGroup::ComputeCollisionGroupTable(const UsdStage& stage)
+{
+    // First, collect all the collision groups, as we want to iterate over them several times
+    std::vector<UsdPhysicsCollisionGroup> allSceneGroups;
+    const UsdPrimRange range(stage.GetPseudoRoot());
+    for (const UsdPrim& prim : range)
+    {
+        if (prim.IsA<UsdPhysicsCollisionGroup>())
+        {
+            allSceneGroups.emplace_back(prim);
+        }
+    }
+
+    // Assign a number to every collision group; in order to handle merge groups, some prims will have non-unique indices
+    std::map<SdfPath, size_t> primPathToIndex; // Using SdfPath, rather than prim, as the filtered groups rel gives us a path.
+    std::map<std::string, size_t> mergeGroupNameToIndex;
+    int nextPrimId = 0;
+
+    for (const UsdPhysicsCollisionGroup& collisionGroup : allSceneGroups)
+    {
+        UsdAttribute mergeGroupAttr = collisionGroup.GetMergeGroupNameAttr();
+
+        // If the group doesn't have a merge group, we can just add it to the table:
+        if (!mergeGroupAttr.IsAuthored())
+        {
+            primPathToIndex[collisionGroup.GetPath()] = nextPrimId;
+            nextPrimId++;
+        }
+        else
+        {
+            std::string mergeGroupName;
+            mergeGroupAttr.Get(&mergeGroupName);
+            auto foundGroup = mergeGroupNameToIndex.find(mergeGroupName);
+            if (foundGroup != mergeGroupNameToIndex.end())
+            {
+                primPathToIndex[collisionGroup.GetPath()] = foundGroup->second;
+            }
+            else
+            {
+                mergeGroupNameToIndex[mergeGroupName] = nextPrimId;
+                primPathToIndex[collisionGroup.GetPath()] = nextPrimId;
+                nextPrimId++;
+            }
+        }
+    }
+
+    // Now, we've seen "nextPrimId" different unique groups after accounting for the merge groups.
+    // Calculate the collision table for those groups.
+
+    // First, resize the table and set to default-collide. We're only going to use the upper diagonal,
+    // as the table is symmetric:
+    std::vector<bool> mergedTable;
+    mergedTable.resize( ((nextPrimId + 1) * nextPrimId) / 2, true);
+
+    for (const UsdPhysicsCollisionGroup& groupA : allSceneGroups)
+    {
+        int groupAIdx = primPathToIndex[groupA.GetPath()];
+
+        // Extract the indices for each filtered group in "prim":
+        std::vector<int> filteredGroupIndices;
+        {
+            UsdRelationship filteredGroups = groupA.GetFilteredGroupsRel();
+            SdfPathVector filteredTargets;
+            filteredGroups.GetTargets(&filteredTargets);
+            for(const SdfPath& path : filteredTargets)
+            {
+                filteredGroupIndices.push_back(primPathToIndex[path]);
+            }
+        }
+
+        bool invertedFilter;
+        UsdAttribute invertedAttr = groupA.GetInvertFilteredGroupsAttr();
+        invertedAttr.Get(&invertedFilter);
+
+        // Now, we are ready to apply the filter rules for "prim":
+        if (!invertedAttr.IsAuthored() || !invertedFilter)
+        {
+            // This is the usual case; collisions against all the filteredTargets should be disabled
+            for (int groupBIdx : filteredGroupIndices)
+            {
+                // Disable aIdx -v- bIdx
+                int minGroup = std::min(groupAIdx, groupBIdx);
+                int maxGroup = std::max(groupAIdx, groupBIdx);
+                int numSkippedEntries = (minGroup * minGroup + minGroup) / 2;
+                mergedTable[minGroup * nextPrimId - numSkippedEntries  + maxGroup] = false;
+            }
+        }
+        else
+        {
+            // This is the less common case; disable collisions against all groups except the filtered targets
+            std::set<int> requestedGroups(filteredGroupIndices.begin(), filteredGroupIndices.end());
+            for(int groupBIdx = 0; groupBIdx < nextPrimId; groupBIdx++)
+            {
+                if (requestedGroups.find(groupBIdx) == requestedGroups.end())
+                {
+                    // Disable aIdx -v- bIdx
+                    int minGroup = std::min(groupAIdx, groupBIdx);
+                    int maxGroup = std::max(groupAIdx, groupBIdx);
+                    int numSkippedEntries = (minGroup * minGroup + minGroup) / 2;
+                    mergedTable[minGroup * nextPrimId - numSkippedEntries + maxGroup] = false;
+                }
+            }
+        }
+    }
+
+    // Finally, we can calculate the output table, based on the merged table
+    CollisionGroupTable res;
+    for (const UsdPhysicsCollisionGroup& collisionGroup : allSceneGroups)
+    {
+        res.groups.push_back(collisionGroup.GetPrim().GetPrimPath());
+    }
+    res.enabled.resize(((res.groups.size() + 1) * res.groups.size()) / 2, true);
+
+    // Iterate over every group A and B, and use the merged table to determine if they collide.
+    for (int iA = 0; iA < allSceneGroups.size(); iA++)
+    {
+        for (int iB = iA; iB < allSceneGroups.size(); iB++)
+        {
+            // Determine if the groups at iA and iB collide:
+            int groupAId = primPathToIndex[allSceneGroups[iA].GetPath()];
+            int groupBId = primPathToIndex[allSceneGroups[iB].GetPath()];
+            int numSkippedMergeEntries = (groupAId * groupAId + groupAId) / 2;
+            bool enabledInMergeTable = mergedTable[groupAId * nextPrimId - numSkippedMergeEntries + groupBId];
+
+            // And use that to populate the output table:
+            int minGroup = std::min(iA, iB);
+            int maxGroup = std::max(iA, iB);
+            int numSkippedEntries = (minGroup * minGroup + minGroup) / 2;
+            res.enabled[minGroup * allSceneGroups.size() - numSkippedEntries + maxGroup] = enabledInMergeTable;
+        }
+    }
+
+    return res;
+}
+
 PXR_NAMESPACE_CLOSE_SCOPE

pxr/usd/usdPhysics/collisionGroup.h

@@ -157,6 +157,56 @@ class UsdPhysicsCollisionGroup : public UsdTyped
     USDPHYSICS_API
     const TfType &_GetTfType() const override;
 
+public:
+    // --------------------------------------------------------------------- //
+    // MERGEGROUPNAME 
+    // --------------------------------------------------------------------- //
+    /// If non-empty, any collision groups in a stage with a matching
+    /// mergeGroup should be considered to refer to the same collection. Matching
+    /// collision groups should behave as if there were a single group containing
+    /// referenced colliders and filter groups from both collections.
+    ///
+    /// | ||
+    /// | -- | -- |
+    /// | Declaration | `string physics:mergeGroup` |
+    /// | C++ Type | std::string |
+    /// | \ref Usd_Datatypes "Usd Type" | SdfValueTypeNames->String |
+    USDPHYSICS_API
+    UsdAttribute GetMergeGroupNameAttr() const;
+
+    /// See GetMergeGroupNameAttr(), and also 
+    /// \ref Usd_Create_Or_Get_Property for when to use Get vs Create.
+    /// If specified, author \p defaultValue as the attribute's default,
+    /// sparsely (when it makes sense to do so) if \p writeSparsely is \c true -
+    /// the default for \p writeSparsely is \c false.
+    USDPHYSICS_API
+    UsdAttribute CreateMergeGroupNameAttr(VtValue const &defaultValue = VtValue(), bool writeSparsely=false) const;
+
+public:
+    // --------------------------------------------------------------------- //
+    // INVERTFILTEREDGROUPS 
+    // --------------------------------------------------------------------- //
+    /// Normally, the filter will disable collisions against the selected
+    /// filter groups. However, if this option is set, the filter will disable
+    /// collisions against all colliders except for those in the selected filter
+    /// groups.
+    ///
+    /// | ||
+    /// | -- | -- |
+    /// | Declaration | `bool physics:invertFilteredGroups` |
+    /// | C++ Type | bool |
+    /// | \ref Usd_Datatypes "Usd Type" | SdfValueTypeNames->Bool |
+    USDPHYSICS_API
+    UsdAttribute GetInvertFilteredGroupsAttr() const;
+
+    /// See GetInvertFilteredGroupsAttr(), and also 
+    /// \ref Usd_Create_Or_Get_Property for when to use Get vs Create.
+    /// If specified, author \p defaultValue as the attribute's default,
+    /// sparsely (when it makes sense to do so) if \p writeSparsely is \c true -
+    /// the default for \p writeSparsely is \c false.
+    USDPHYSICS_API
+    UsdAttribute CreateInvertFilteredGroupsAttr(VtValue const &defaultValue = VtValue(), bool writeSparsely=false) const;
+
 public:
     // --------------------------------------------------------------------- //
     // FILTEREDGROUPS 
@@ -188,6 +238,34 @@ class UsdPhysicsCollisionGroup : public UsdTyped
     /// what colliders belong to the CollisionGroup.
     USDPHYSICS_API
     UsdCollectionAPI GetCollidersCollectionAPI() const;    
+
+    // Utility structure generated by ComputeCollisionGroupTable (); contains a table describing
+    // which pairs of collision groups have collisions enabled/disabled by the filtering rules.
+    struct CollisionGroupTable
+    {
+        /// Return the set of all UsdPhysicsCollisionGroup which this table contains
+        USDPHYSICS_API
+        const SdfPathVector& GetCollisionGroups() const;
+
+        /// Return true if the groups at indices \a idxA and \a idxB collide
+        USDPHYSICS_API
+        bool IsCollisionEnabled(int idxA, int idxB) const;
+
+        /// Return true if the groups \a primA and \a primB collide
+        USDPHYSICS_API
+        bool IsCollisionEnabled(const SdfPath& primA, const SdfPath& primB) const;
+
+    protected:
+        friend class UsdPhysicsCollisionGroup;
+        SdfPathVector groups; //< All collision groups known to this table
+        std::vector<bool> enabled; //< 2D table, with one element per collision-group-pair. Entry is false if collision is disabled by a filtering rule.
+    };
+
+    /// Compute a table encoding all the collision groups filter rules for a stage. This can be used
+    /// as a reference to validate an implementation of the collision groups filters. The returned
+    /// table is diagonally symmetric.
+    static USDPHYSICS_API
+    CollisionGroupTable ComputeCollisionGroupTable(const UsdStage& stage);
 };
 
 PXR_NAMESPACE_CLOSE_SCOPE

pxr/usd/usdPhysics/generatedSchema.usda

@@ -203,6 +203,20 @@ class PhysicsCollisionGroup "PhysicsCollisionGroup" (
         doc = """References a list of PhysicsCollisionGroups with which 
         collisions should be ignored."""
     )
+    bool physics:invertFilteredGroups (
+        displayName = "Invert Filtered Groups"
+        doc = """Normally, the filter will disable collisions against the selected
+        filter groups. However, if this option is set, the filter will disable
+        collisions against all colliders except for those in the selected filter
+        groups."""
+    )
+    string physics:mergeGroup (
+        displayName = "Merge With Groups"
+        doc = """If non-empty, any collision groups in a stage with a matching
+        mergeGroup should be considered to refer to the same collection. Matching
+        collision groups should behave as if there were a single group containing
+        referenced colliders and filter groups from both collections."""
+    )
 }
 
 class "PhysicsFilteredPairsAPI" (