void copyFromFullToReduced( vectorType & reducedVector,
const vectorType & fullVector,
const Model & reducedModel,
const Model & fullModel)
{
for(JointIndex jntReduced = 0; jntReduced < reducedModel.getNrOfJoints(); jntReduced++ )
{
IJointConstPtr jnt = reducedModel.getJoint(jntReduced);
std::string jointName = reducedModel.getJointName(jntReduced);
if( jnt->getNrOfDOFs() > 0 )
{
IJointConstPtr jntInFullModel = fullModel.getJoint(fullModel.getJointIndex(jointName));
reducedVector(jnt->getPosCoordsOffset()) = fullVector(jntInFullModel->getPosCoordsOffset());
}
}
}
void copyFromReducedToFull(const vectorType & reducedVector,
vectorType & fullVector,
const Model & reducedModel,
const Model & fullModel)
{
for(JointIndex jntReduced = 0; jntReduced < reducedModel.getNrOfJoints(); jntReduced++ )
{
IJointConstPtr jnt = reducedModel.getJoint(jntReduced);
std::string jointName = reducedModel.getJointName(jntReduced);
if( jnt->getNrOfDOFs() > 0 )
{
IJointConstPtr jntInFullModel = fullModel.getJoint(fullModel.getJointIndex(jointName));
assert(jntInFullModel->getNrOfDOFs() > 0);
assert(fullVector.size() == fullModel.getNrOfPosCoords());
assert(reducedVector.size() == reducedModel.getNrOfPosCoords());
assert(jntInFullModel->getPosCoordsOffset() < fullVector.size());
assert(jnt->getPosCoordsOffset() < reducedVector.size());
fullVector(jntInFullModel->getPosCoordsOffset()) = reducedVector(jnt->getPosCoordsOffset());
}
}
}