void ComputeParentIndices(const URDFImporterInterface& u2b, URDF2BulletCachedData& cache, int urdfLinkIndex, int urdfParentIndex)
{
cache.m_urdfLinkParentIndices[urdfLinkIndex]=urdfParentIndex;
cache.m_urdfLinkIndices2BulletLinkIndices[urdfLinkIndex]=cache.m_currentMultiBodyLinkIndex++;
btAlignedObjectArray<int> childIndices;
u2b.getLinkChildIndices(urdfLinkIndex,childIndices);
for (int i=0;i<childIndices.size();i++)
{
ComputeParentIndices(u2b,cache,childIndices[i],urdfLinkIndex);
}
}
void InitURDF2BulletCache(const URDFImporterInterface& u2b, URDF2BulletCachedData& cache, int flags)
{
//compute the number of links, and compute parent indices array (and possibly other cached data?)
cache.m_totalNumJoints1 = 0;
int rootLinkIndex = u2b.getRootLinkIndex();
if (rootLinkIndex >= 0)
{
ComputeTotalNumberOfJoints(u2b, cache, rootLinkIndex);
int numTotalLinksIncludingBase = 1 + cache.m_totalNumJoints1;
cache.m_urdfLinkParentIndices.resize(numTotalLinksIncludingBase);
cache.m_urdfLinkIndices2BulletLinkIndices.resize(numTotalLinksIncludingBase);
cache.m_urdfLink2rigidBodies.resize(numTotalLinksIncludingBase);
cache.m_urdfLinkLocalInertialFrames.resize(numTotalLinksIncludingBase);
cache.m_currentMultiBodyLinkIndex = -1; //multi body base has 'link' index -1
bool maintainLinkOrder = (flags & CUF_MAINTAIN_LINK_ORDER)!=0;
if (maintainLinkOrder)
{
URDF2BulletCachedData cache2 = cache;
ComputeParentIndices(u2b, cache2, rootLinkIndex, -2);
for (int j=0;j<numTotalLinksIncludingBase;j++)
{
cache.m_urdfLinkParentIndices[j] = cache2.m_urdfLinkParentIndices[j];
cache.m_urdfLinkIndices2BulletLinkIndices[j] = j - 1;
}
}else
{
ComputeParentIndices(u2b, cache, rootLinkIndex, -2);
}
}
}
void InitURDF2BulletCache(const URDFImporterInterface& u2b, URDF2BulletCachedData& cache)
{
//compute the number of links, and compute parent indices array (and possibly other cached data?)
cache.m_totalNumJoints1 = 0;
int rootLinkIndex = u2b.getRootLinkIndex();
if (rootLinkIndex>=0)
{
ComputeTotalNumberOfJoints(u2b,cache,rootLinkIndex);
int numTotalLinksIncludingBase = 1+cache.m_totalNumJoints1;
cache.m_urdfLinkParentIndices.resize(numTotalLinksIncludingBase);
cache.m_urdfLinkIndices2BulletLinkIndices.resize(numTotalLinksIncludingBase);
cache.m_urdfLink2rigidBodies.resize(numTotalLinksIncludingBase);
cache.m_urdfLinkLocalInertialFrames.resize(numTotalLinksIncludingBase);
cache.m_currentMultiBodyLinkIndex = -1;//multi body base has 'link' index -1
ComputeParentIndices(u2b,cache,rootLinkIndex,-2);
}
}
