const Part::TopoShape Feature::getBaseTopoShape() const {
const Part::Feature* BaseObject = getBaseObject();
const Part::TopoShape& result = BaseObject->Shape.getShape();
if (result._Shape.IsNull())
throw Base::Exception("Base feature's TopoShape is invalid");
return result;
}
int CIKGraphObjCont::identifyElements()
{ // returns the number of active joints
if (container.size()==0)
return(0);
for (int i=0;i<int(container.size());i++)
{
CIKGraphNode* it=container[i];
it->elementID=-1;
}
CIKGraphNode* iterat=getBaseObject();
if (iterat==NULL)
{
iterat=container[0];
setBaseObject(container[0]->nodeID);
}
int currentElementID=0;
std::vector<CIKGraphNode*> startOfNewElement;
startOfNewElement.push_back(iterat);
int activeJointCount=0;
while (startOfNewElement.size()!=0)
{
iterat=startOfNewElement.back();
startOfNewElement.pop_back();
if (iterat->elementID==-1)
{
std::vector<CIKGraphNode*> toExplore;
toExplore.push_back(iterat);
while (toExplore.size()!=0)
{
iterat=toExplore.back();
toExplore.pop_back();
iterat->elementID=currentElementID;
if (iterat->type==IK_GRAPH_OBJECT_TYPE)
{
CIKGraphObject* it=(CIKGraphObject*)iterat;
if (it->linkPartner!=NULL)
startOfNewElement.push_back(it->linkPartner);
}
else
activeJointCount++;
int neighNB=iterat->getConnectionNumber();
for (int i=0;i<neighNB;i++)
{
CIKGraphNode* candidate=iterat->getNeighbour(i,true);
if ( (candidate!=NULL)&&(candidate->elementID==-1) )
toExplore.push_back(candidate);
}
}
currentElementID++;
}
}
numberOfElementIDs=currentElementID;
return(activeJointCount);
}
const TopoDS_Shape& Feature::getBaseShape() const {
const Part::Feature* BaseObject = getBaseObject();
const TopoDS_Shape& result = BaseObject->Shape.getValue();
if (result.IsNull())
throw Base::Exception("Base feature's shape is invalid");
TopExp_Explorer xp (result, TopAbs_SOLID);
if (!xp.More())
throw Base::Exception("Base feature's shape is not a solid");
return result;
}
void CIKGraphObjCont::actualizeTransformationsWithElementID(int elementID)
{
CIKGraphNode* it=NULL;
if (elementID==0)
it=getBaseObject();
else
{
for (int i=0;i<int(container.size());i++)
{
if ( (container[i]->elementID==elementID)&&(container[i]->type==IK_GRAPH_OBJECT_TYPE) )
{
it=container[i];
break;
}
}
}
C7Vector prevToCurrTr;
prevToCurrTr.setIdentity();
std::vector<C7Vector> prevToCurrTrs;
prevToCurrTrs.push_back(prevToCurrTr);
CIKGraphNode* prevNode=NULL;
std::vector<CIKGraphNode*> prevNodes;
prevNodes.push_back(prevNode);
CIKGraphNode* currentNode=it;
std::vector<CIKGraphNode*> currentNodes;
currentNodes.push_back(currentNode);
while (currentNodes.size()!=0)
{
prevToCurrTr=prevToCurrTrs.back();
prevToCurrTrs.pop_back();
prevNode=prevNodes.back();
prevNodes.pop_back();
currentNode=currentNodes.back();
currentNodes.pop_back();
C7Vector futurePrevOldTr;
if (currentNode->explorationID==-1)
{
if ( (prevNode!=NULL)&&(prevNode->type==IK_GRAPH_JOINT_TYPE) )
{ // We have : joint--->object
//1. We prepare future explorations first:
int i=0;
while (currentNode->getUnexplored(i)!=NULL)
{
CIKGraphNode* aNode=currentNode->getUnexplored(i);
C7Vector currentCTMI(((CIKGraphObject*)currentNode)->cumulativeTransformation.getInverse());
C7Vector nextCTM(((CIKGraphObject*)aNode)->cumulativeTransformation);
prevToCurrTrs.push_back(currentCTMI*nextCTM);
prevNodes.push_back(currentNode);
currentNodes.push_back(aNode);
i++;
}
//2. We handle this object:
currentNode->explorationID=0; // We flag this
CIKGraphJoint* prevJoint=(CIKGraphJoint*)prevNode;
if (prevJoint->topObject==currentNode)
prevJoint->topObject->cumulativeTransformation=prevJoint->downObject->cumulativeTransformation*prevJoint->getDownToTopTransformation();
else
prevJoint->downObject->cumulativeTransformation=prevJoint->topObject->cumulativeTransformation*prevJoint->getDownToTopTransformation().getInverse();
}
else if (currentNode->type==IK_GRAPH_JOINT_TYPE)
{ // We have : object--->joint
//1. We prepare future explorations first:
int i=0;
while (currentNode->getUnexplored(i)!=NULL)
{
CIKGraphNode* aNode=currentNode->getUnexplored(i);
prevToCurrTrs.push_back(prevToCurrTr); // The content doesn't matter here
prevNodes.push_back(currentNode);
currentNodes.push_back(aNode);
i++;
}
//2. We handle this object:
currentNode->explorationID=0; // We flag this
}
else
{ // We have : object--->object or NULL--->object
//1. We prepare future explorations first:
int i=0;
while (currentNode->getUnexplored(i)!=NULL)
{
CIKGraphNode* aNode=currentNode->getUnexplored(i);
C7Vector currentCTMI(((CIKGraphObject*)currentNode)->cumulativeTransformation.getInverse());
C7Vector nextCTM;
if (aNode->type==IK_GRAPH_OBJECT_TYPE)
{
nextCTM=(((CIKGraphObject*)aNode)->cumulativeTransformation);
// In case aNode is a joint type, nextCTM can be anything (above)
}
if (aNode->elementID==elementID)
{ // We follow only same elementIDs!
prevToCurrTrs.push_back(currentCTMI*nextCTM);
prevNodes.push_back(currentNode);
currentNodes.push_back(aNode);
}
i++;
}
//2. We handle this object:
currentNode->explorationID=0; // We flag this
if (prevNode!=NULL)
((CIKGraphObject*)currentNode)->cumulativeTransformation=((CIKGraphObject*)prevNode)->cumulativeTransformation*prevToCurrTr;
}
}
}
}
