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); } }