void ConnectivityGraph::setAsReplacement( EdgeIP e, ETForestLevel l ) {
assert( e->level == l );
assert( !e->isTree );
Vertex & u( vertices[e->vxid1] );
Vertex & v( vertices[e->vxid2] );
// remove from non-tree list and update weights
removeNontreeEdge( e.get(), l );
e->isTree = true;
// std::cout << "adding edge " << *e << " as replacement, giving " << std::endl;
for ( int i = 0; i <= l; ++i ) {
// get the previous roots on level l
ETNode * ru = u.ETvertices[i].loopNode->findRoot();
ETNode * rv = v.ETvertices[i].loopNode->findRoot();
assert( ru != rv );
// remove the previous roots
removeRoot( ru, i );
removeRoot( rv, i );
// add the edge to this level (and the forest)
ETNode * newroot = addTreeEdge( u, v, e, i );
// remember the new root
insertRoot( newroot, i );
}
// EulerTourTree::print( u.ETvertices[0].loopNode->findRoot() );
// std::cout << std::endl;
}
//void ConnectivityGraph::insertEdge( VariableID vid, FactorID fid ) {
void ConnectivityGraph::insertEdge( VertexID vxid1, VertexID vxid2 ) {
assert( vxid1 != vxid2 );
Vertex & u( vertices[vxid1] );
Vertex & v( vertices[vxid2] );
ETNode * ru = u.ETvertices[0].loopNode->findRoot();
ETNode * rv = v.ETvertices[0].loopNode->findRoot();
EdgeIP e = edgepool.get();
e->set( /*vid, fid,*/ vxid1, vxid2, /*0,*/ 0 );
// std::cout << "inserting/incrementing edge: " << *e << std::endl;
//#ifdef DEBUG
// assert( !existingEdges[vxid][fxid][fid] );
// assert( !existingEdges[fxid][vxid][fid] );
// existingEdges[vxid][fxid].set( fid );
// existingEdges[fxid][vxid].set( fid );
//#endif // DEBUG
// if ( e->count == 0 ) {
// e->set( vid, fid, vxid, fxid, 1, 0 ); // ensure level is 0
// std::cout << "adding edge to graph: " << *e << std::endl;
// check if they're already connected
if ( ru == rv ) {
// add edge to non-tree adjacency lists and update weights, which
// correspond to edge counts
addNontreeEdge( u, v, e, 0 );
} else {
e->isTree = true;
// find and remove the two previous roots from the level 0 forest
removeRoot( ru, 0 );
removeRoot( rv, 0 );
ETNode * newroot = addTreeEdge( u, v, e, 0 );
// put the new root into the level 0 forest -- note that we use the
// root's vertex ID as the index
insertRoot( newroot, 0 );
}
// } else {
// ++e->count;
// }
}
taoNodeRoot* taoGroup::unlinkFixed(taoNodeRoot* root, taoNode* node)
{
#ifdef TAO_CONTROL
if (root->getController())
root->getController()->deleteParamTree(node);
#endif
taoNodeRoot* r = new taoNodeRoot(*node->frameGlobal());
node->unlink();
if (!root->getDChild())
delete removeRoot(root->getID());
deFrame home;
home.identity();
node->link(r, &home);
node->addABNode();
taoDynamics::initialize(r);
addRoot(r, -999);
return r;
}
void ConnectivityGraph::raiseTreeEdges( ETNode * n, ETForestLevel l ) {
assert( n->isLoop() );
Vertex & u( vertices[n->vx1->vxid] );
auto & list = u.adjTreeEdges[l];
while ( !list.empty() ) {
// get and remove the last edge (efficient in a vector)
EdgeIP e = list.back();
assert( e->isTree );
assert( e->level == l );
// removeTreeEdge( e.get(), l );
list.pop_back();
Vertex & v( e->vxid1 == n->vx1->vxid ? vertices[e->vxid2] :
vertices[e->vxid1] );
assert( EulerTourTree::connected( &u.ETvertices[l], &v.ETvertices[l] ) );
// remove e from v's adjacency lists
EdgeIP ev = getFromList( e->vxid1, e->vxid2, v.adjTreeEdges[l], l );
assert( ev == e );
u.ETvertices[l].loopNode->addWeight( -1, ETWeight::Tree );
v.ETvertices[l].loopNode->addWeight( -1, ETWeight::Tree );
// get the previous roots on level l+1
ETNode * ru = u.ETvertices[l+1].loopNode->findRoot();
ETNode * rv = v.ETvertices[l+1].loopNode->findRoot();
assert( ru != rv );
// remove the previous roots
removeRoot( ru, l+1 );
removeRoot( rv, l+1 );
// move the edge up a level, add it to this new level (and the forest)
e->level = l + 1;
ETNode * newroot = addTreeEdge( u, v, e, l+1 );
// remember the new root
insertRoot( newroot, l+1 );
}
}
/**
* @brief removes an entry from the TBST
* @details removes the specified entry from the TBST if
* it exists, and restructures the tree. Running time O(logn).
*
* @param token entry value to remove
* @return true if the entry is found and removed
* otherwise false.
*/
bool TBST::remove(string token) {
if (root != NULL && root->getData()->getToken() == token) {
if(removeRoot()) {
nodeCount--;
return true;
} else return false;
} else if (root != NULL) {
if(remove(root, token)) {
nodeCount--;
return true;
} else return false;
} else {
return false;
}
}
heap* getData(char *file){
FILE* input_file = fopen(file,"r");
if(input_file == NULL){
printf("Error opening input file %s\n",file);
exit(1);
}
float inputx = 0.0;
float inputy = 0.0;
heap *newheap = createHeap();
int i = 0;
while(fscanf(input_file, "%f", &inputx) != EOF){
fscanf(input_file, "%f", &inputy);
data *d= createData(inputx,inputy);
d->distance = calculateDistance(d->valx,d->valy);
if(i>9){
if(d->distance < newheap->root->d->distance){
leaf *removeleaf = removeRoot(newheap);
#if DEBUG
printf("deleted : %f and inserted : %f -- \n",removeleaf->d->distance,d->distance);
#endif
popBack(newheap->q->ll);
free(removeleaf->d);
free(removeleaf);
addLeaf(newheap,d);
} else {
#if DEBUG
printf("Skipped : %f => ",d->distance);
printf("deleting ...... \n");
#endif
free(d);
}
}
else {
#if DEBUG
printf("inserted : %f -- \n",d->distance);
#endif
addLeaf(newheap,d);
}
i++;
}
fclose(input_file);
return newheap;
}
void ConnectivityGraph::deleteEdge( EdgeIP e ) {
// std::cout << "deleting edge " << *e <<
// ( e->isTree ? " (tree)" : " (nontree)" ) << std::endl;
// assert( e->count == 0 );
Vertex & u( vertices[e->vxid1] );
Vertex & v( vertices[e->vxid2] );
assert( EulerTourTree::connected( &u.ETvertices[0], &v.ETvertices[0] ) );
if ( e->isTree ) {
// cut all forests F_i, for 0 <= i <= level(e)
for ( ETForestLevel i = e->level; i >= 0; --i ) {
ETNode * oldroot = e->arcs[i].first->findRoot();
assert( oldroot == e->arcs[i].second->findRoot() );
// remove the old root from the forest
removeRoot( oldroot, i );
ETNode * r1 = NULL, * r2 = NULL;
EulerTourTree::cut( e->arcs[i].first, e->arcs[i].second, &r1, &r2 );
#ifdef DEBUG
ETNode * ru = u.ETvertices[i].loopNode->findRoot();
ETNode * rv = v.ETvertices[i].loopNode->findRoot();
assert( ru != rv );
assert( ru == r1 || ru == r2 );
assert( rv == r1 || rv == r2 );
#endif
// put the new roots in the forest
insertRoot( r1, i );
insertRoot( r2, i );
}
// attempt to find a replacement edge
for ( ETForestLevel i = e->level; i >= 0; --i ) {
if ( replaceEdge( *e, i ) ) break;
}
}
// delete e;
}
void printHeap(heap *h){
data **listCor = (data**)malloc(sizeof(data*)*10);
int i = 0;
while(h->root){
if(i>9){
popBack(h->q->ll);
break;
}
leaf *rootleaf = removeRoot(h);
#if DEBUG
printf("removing data %f at %d \n",rootleaf->d->distance,i);
#endif
listCor[i] = createData(rootleaf->d->valx,rootleaf->d->valy);
listCor[i]->distance = calculateDistance(listCor[i]->valx,listCor[i]->valy);
if(h->root == rootleaf){
free(h->root->d);
free(h->root);
h->root = NULL;
} else {
free(rootleaf->d);
free(rootleaf);
}
popBack(h->q->ll);
i++;
}
for(i = 9;i>=0;i--){
printf(" %.2f %.2f %.2f \n",listCor[i]->distance,listCor[i]->valx,listCor[i]->valy);
data *d = createData(listCor[i]->valx,listCor[i]->valy);
d->distance = calculateDistance(d->valx,d->valy);
addLeaf(h,d);
free(listCor[i]);
}
free(listCor);
}
taoNodeRoot* taoGroup::unlinkFree(taoNodeRoot* root, taoNode* node, deFloat inertia, deFloat damping)
{
#ifdef TAO_CONTROL
if (root->getController())
root->getController()->deleteParamTree(node);
#endif
deVector6 v = *node->velocity();
taoNodeRoot* r = new taoNodeRoot(*node->frameGlobal());
node->unlink();
if (!root->getDChild())
delete removeRoot(root->getID());
deFrame home;
home.identity();
node->link(r, &home);
taoJointDOF1* joint;
joint = new taoJointPrismatic(TAO_AXIS_X);
joint->setDamping(damping);
joint->setInertia(inertia);
joint->setDVar(new taoVarDOF1);
joint->reset();
joint->getVarDOF1()->_dQ = v[0][TAO_AXIS_X];
node->addJoint(joint);
joint = new taoJointPrismatic(TAO_AXIS_Y);
joint->setDamping(damping);
joint->setInertia(inertia);
joint->setDVar(new taoVarDOF1);
joint->reset();
joint->getVarDOF1()->_dQ = v[0][TAO_AXIS_Y];
node->addJoint(joint);
joint = new taoJointPrismatic(TAO_AXIS_Z);
joint->setDamping(damping);
joint->setInertia(inertia);
joint->setDVar(new taoVarDOF1);
joint->reset();
joint->getVarDOF1()->_dQ = v[0][TAO_AXIS_Z];
node->addJoint(joint);
taoJointSpherical* joint2 = new taoJointSpherical;
joint2->setDamping(damping);
joint2->setInertia(inertia);
joint2->setDVar(new taoVarSpherical);
joint2->reset();
joint2->getVarSpherical()->_dQ = v[1];
node->addJoint(joint2);
node->addABNode();
taoDynamics::initialize(r);
addRoot(r, -999);
return r;
}
