Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
void printCache(){
gArticulation->releaseCache(*articulationCache);
articulationCache= gArticulation->createCache();
const int maxLinks = 64;
PxArticulationLink* links[maxLinks];
PxU32 TotalLinkCount = gArticulation->getLinks(links, maxLinks, 0);
std::cout << "Total links: " << TotalLinkCount << std::endl;
PxU32 dofStarts[maxLinks];
dofStarts[0] = 0; //We know that the root link does not have a joint
for(PxU32 i = 1; i < TotalLinkCount; ++i)
{
PxU32 llIndex = links[i]->getLinkIndex();
PxU32 dofs = links[i]->getInboundJointDof();
dofStarts[llIndex] = dofs;
}
PxU32 count = 0;
for(PxU32 i = 1; i < TotalLinkCount; ++i)
{
PxU32 dofs = dofStarts[i];
dofStarts[i] = count;
count += dofs;
}
gArticulation->copyInternalStateToCache(*articulationCache, PxArticulationCache::eALL);
// Print position, velocity and acceleration of each DOF
for(int ii = 0; ii < (int)TotalLinkCount; ii++){
int nDOF = links[ii]->getInboundJointDof();
printf("Link: %02d, #DOF: %d\r\n",ii, nDOF);
for(int jj = 0; jj<(int)nDOF; jj++){
PxReal pos = articulationCache->jointPosition[dofStarts[links[ii]->getLinkIndex()]+jj];
PxReal vel = articulationCache->jointVelocity[dofStarts[links[ii]->getLinkIndex()]+jj];
PxReal acc = articulationCache->jointAcceleration[dofStarts[links[ii]->getLinkIndex()]+jj];
printf("DOF#: %02d, pos: %05.6f, \t vel: %05.6f, \t acc: %05.6f\r\n",jj,(double)pos,(double)vel,(double)acc);
}
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment