void ComputeTotalNumberOfJoints(const URDFImporterInterface& u2b, URDF2BulletCachedData& cache, int linkIndex)
{
btAlignedObjectArray<int> childIndices;
u2b.getLinkChildIndices(linkIndex,childIndices);
//b3Printf("link %s has %d children\n", u2b.getLinkName(linkIndex).c_str(),childIndices.size());
//for (int i=0;i<childIndices.size();i++)
//{
// b3Printf("child %d has childIndex%d=%s\n",i,childIndices[i],u2b.getLinkName(childIndices[i]).c_str());
//}
cache.m_totalNumJoints1 += childIndices.size();
for (int i=0;i<childIndices.size();i++)
{
int childIndex =childIndices[i];
ComputeTotalNumberOfJoints(u2b,cache,childIndex);
}
}
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);
}
}
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);
}
}
}