Skip to content

Commit

Permalink
Merge pull request #1947 from eoineoineoin/PhysicsFilteringGroups
Browse files Browse the repository at this point in the history 
Physics: Add support for merging collision groups.

(Internal change: 2248210)
  • Loading branch information
@pixar-oss
pixar-oss committed Sep 7, 2022
2 parents f450728 + b1ae40d commit a024497
Show file tree
Hide file tree
Showing 11 changed files with 716 additions and 9 deletions.
6 changes: 6 additions & 0 deletions pxr/usd/usdPhysics/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -91,6 +91,7 @@ pxr_library(usdPhysics
pxr_test_scripts(
testenv/testUsdPhysicsMetrics.py
testenv/testUsdPhysicsRigidBodyAPI.py
testenv/testUsdPhysicsCollisionGroupAPI.py
)

pxr_register_test(testUsdPhysicsMetrics
Expand All @@ -105,3 +106,8 @@ pxr_register_test(testUsdPhysicsRigidBodyAPI
EXPECTED_RETURN_CODE 0
)

pxr_register_test(testUsdPhysicsCollisionGroupAPI
PYTHON
COMMAND "${CMAKE_INSTALL_PREFIX}/tests/testUsdPhysicsCollisionGroupAPI"
EXPECTED_RETURN_CODE 0
)
261 changes: 259 additions & 2 deletions pxr/usd/usdPhysics/collisionGroup.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand All @@ -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;
Expand All @@ -141,13 +192,219 @@ PXR_NAMESPACE_CLOSE_SCOPE
// ===================================================================== //
// --(BEGIN CUSTOM CODE)--

#include "pxr/usd/usd/primRange.h"
#include <algorithm>

PXR_NAMESPACE_OPEN_SCOPE

// Helper private function to get index into the collision table
// Given indices of the 2 collision groups idxA < idxB and current group size
static
unsigned int
_GetCollisionTableIndex(const unsigned int idxA, const unsigned int idxB,
const unsigned int groupSize)
{
const unsigned int numSkippedEntries = (idxA * idxA + idxA) / 2;
return (idxA * groupSize - numSkippedEntries + idxB);
}

UsdCollectionAPI
UsdPhysicsCollisionGroup::GetCollidersCollectionAPI() const
{
return UsdCollectionAPI(GetPrim(), UsdPhysicsTokens->colliders);
}

const SdfPathVector&
UsdPhysicsCollisionGroup::CollisionGroupTable::GetCollisionGroups() const
{
return _groups;
}

bool
UsdPhysicsCollisionGroup::CollisionGroupTable::IsCollisionEnabled(
const unsigned int idxA, const unsigned int idxB) const
{
// check if indices are legal
// - both idxA and idxB are by definition >=0
// - are < the group size.
if (idxA < _groups.size() && idxB < _groups.size())
{
const unsigned int minGroup = std::min(idxA, idxB);
const unsigned int maxGroup = std::max(idxA, idxB);
const unsigned int tableIndex =
_GetCollisionTableIndex(minGroup, maxGroup, _groups.size());
return _enabled[tableIndex];
}

// 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
// Using SdfPath, rather than prim, as the filtered groups rel gives us a
// path.
std::map<SdfPath, size_t> primPathToIndex;
std::map<std::string, size_t> mergeGroupNameToIndex;
unsigned 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)
{
unsigned int groupAIdx = primPathToIndex[groupA.GetPath()];

// Extract the indices for each filtered group in "prim":
std::vector<unsigned int> filteredGroupIndices;
{
UsdRelationship filteredGroups = groupA.GetFilteredGroupsRel();
SdfPathVector filteredTargets;
filteredGroups.GetTargets(&filteredTargets);
filteredGroupIndices.reserve(filteredTargets.size());
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 (unsigned int groupBIdx : filteredGroupIndices)
{
// Disable aIdx -v- bIdx
const unsigned int minGroup = std::min(groupAIdx, groupBIdx);
const unsigned int maxGroup = std::max(groupAIdx, groupBIdx);
const unsigned int tableIndex =
_GetCollisionTableIndex(minGroup, maxGroup, nextPrimId);
mergedTable[tableIndex] = false;
}
}
else
{
// This is the less common case; disable collisions against all
// groups except the filtered targets
std::set<unsigned int> requestedGroups(
filteredGroupIndices.begin(), filteredGroupIndices.end());
for(unsigned int groupBIdx = 0; groupBIdx < nextPrimId; groupBIdx++)
{
if (requestedGroups.find(groupBIdx) == requestedGroups.end())
{
// Disable aIdx -v- bIdx
const unsigned int minGroup = std::min(groupAIdx, groupBIdx);
const unsigned int maxGroup = std::max(groupAIdx, groupBIdx);
const unsigned int tableIndex =
_GetCollisionTableIndex(minGroup, maxGroup, nextPrimId);
mergedTable[tableIndex] = false;
}
}
}
}

// Finally, we can calculate the output table, based on the merged table
CollisionGroupTable res;
res._groups.reserve(allSceneGroups.size());
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 (unsigned int iA = 0; iA < allSceneGroups.size(); iA++)
{
for (unsigned int iB = iA; iB < allSceneGroups.size(); iB++)
{
// Determine if the groups at iA and iB collide:
const unsigned int groupAId =
primPathToIndex[allSceneGroups[iA].GetPath()];
const unsigned int groupBId =
primPathToIndex[allSceneGroups[iB].GetPath()];
const unsigned int mergedTableIndex =
_GetCollisionTableIndex(groupAId, groupBId, nextPrimId);
bool enabledInMergeTable = mergedTable[mergedTableIndex];

// And use that to populate the output table:
const unsigned int minGroup = std::min(iA, iB);
const unsigned int maxGroup = std::max(iA, iB);
const unsigned int tableIndex =
_GetCollisionTableIndex(minGroup, maxGroup,
allSceneGroups.size());
res._enabled[tableIndex] = enabledInMergeTable;
}
}

return res;
}

PXR_NAMESPACE_CLOSE_SCOPE
Loading

0 comments on commit a024497

Please sign in to comment.