std::vector<RobotNodePtr> RobotNode::getAllParents( RobotNodeSetPtr rns )
{
std::vector<RobotNodePtr> result;
if (!rns)
return result;
std::vector<RobotNodePtr> rn = rns->getAllRobotNodes();
for (unsigned int i=0; i<rn.size(); i++)
{
if (rn[i]->hasChild(static_pointer_cast<SceneObject>(shared_from_this()),true))
result.push_back(rn[i]);
}
return result;
}
CoMIK::CoMIK(RobotNodeSetPtr rnsJoints, RobotNodeSetPtr rnsBodies, RobotNodePtr coordSystem, int dimensions)
: JacobiProvider(rnsJoints), coordSystem(coordSystem)
{
VR_ASSERT(rns);
VR_ASSERT(rnsBodies);
checkImprovement = false;
this->rnsBodies = rnsBodies;
numDimensions = dimensions;
bodyNodes = rnsBodies->getAllRobotNodes();
for (size_t i = 0; i < bodyNodes.size(); i++)
{
// get all joints that influence the body
std::vector<RobotNodePtr> parentsN = bodyNodes[i]->getAllParents(rns);
bodyNodeParents[bodyNodes[i]] = parentsN;
}
if (rnsBodies->getMass()==0)
{
VR_ERROR << "The RNS does not contain any bodies or masses are not specified (mass==0)" << endl;
}
}
