AnimaL

Tutorial

Documentation

---

Documentation

• Overview
• Modules
• Namespace List
• Class Hierarchy
• Alphabetical List
• Compound List
• File List
• Namespace Members
• Compound Members
• File Members
• Examples

---

ConstrainedVertex.cpp

Go to the documentation of this file.

/***************************************************************************
                          ConstrainedVertex.cpp  -  description
                             -------------------
    begin                : Wed Jan 14 2004
    copyright            : (C) 2004 by Mathieu Coquerelle
    email                : mathieu.coquerelle@imag.fr
 ***************************************************************************/

/***************************************************************************
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 ***************************************************************************/

#include "ConstrainedVertex.h"
#include "octree_instanciation.h"

#include <set>
#include "SFVec3fCellConstrained.h"
#include <iostream>
using std::cerr;
using std::endl;

#include "Octree.h"


namespace animal
{
namespace octree
{
ConstrainedVertex::ConstrainedVertex(   Vec3d position, CVertex *cv1, CVertex *cv2, CVertex *cv3, CVertex *cv4,
                                      CVertex *cv5, CVertex *cv6, CVertex *cv7, CVertex *cv8,
                                      Cell *connectedCell ) :
        _position(position),
        _velocity(0,0,0),
        _mass(0),
        _fatherCell(connectedCell), _mainCell(NULL), _geoLink(NULL)
    , _lastConstrainedDepth(0)
{

    std::vector<ConstrainedVertex*> parents(8);
    std::vector<unsigned short> empty(0);
    parents[0] = cv1;
    parents[1] = cv2;
    parents[2] = cv3;
    parents[3] = cv4;
    parents[4] = cv5;
    parents[5] = cv6;
    parents[6] = cv7;
    parents[7] = cv8;

    _geoLink = new GeoLink( parents, empty, true, this );
    //updateFrame();
    
    Ensure( isFree() );
}

// ROOT
ConstrainedVertex::ConstrainedVertex( Vec3d position,  Cell *connectedCell ) :
        _position(position),
        _velocity(0,0,0),
        _mass(0),
        _fatherCell(connectedCell), _mainCell(NULL), _geoLink(NULL)
    ,_lastConstrainedDepth(0)
{

    Ensure( (isFree()) && (this->get_Vec3d() == position) );
    Ensure( nChildren() == 0 );
    
    //updateFrame();
}


ConstrainedVertex::ConstrainedVertex( FloatingPointType x, FloatingPointType y, FloatingPointType z, Cell *connectedCell ) :
        _position(Vec3d(x,y,z)),
        _velocity(0,0,0),
        _mass(0),
        _fatherCell(connectedCell), _mainCell(NULL), _geoLink(NULL)
    ,_lastConstrainedDepth(0)
{
    //updateFrame();
    Ensure( (isFree()) && (this->get_Vec3d() == Vec3d(x,y,z)) );
    Ensure( nChildren() == 0 );
}


ConstrainedVertex::ConstrainedVertex( const ConstrainedVertex & cmsv )
{
    copyAllData( cmsv );
}


ConstrainedVertex & ConstrainedVertex ::operator=( const ConstrainedVertex & cmsv )
{
    copyAllData( cmsv );
    return *this;
}





ConstrainedVertex::~ConstrainedVertex()
{
    Require( nChildren() == 0 );
    if( nChildren() != 0 )
    {
        std::cerr << "This is " << this << " of depth " << this->getDepth() << "\n";
        std::cerr << *(getMainCell()) << "\n";
        std::cerr << "My children are :\n";
        for( unsigned int i=0 ; i<nChildren(); ++i )
        {
            std::cerr << "\tchild["<<i<<"] = " << child(i) << " of depth " << child(i)->getDepth() << "\n";
            std::cerr << "\t\tIt's main cell is " << child(i)->getMainCell() << " of depth " << child(i)->getMainCell()->getData()._depth << "and vId is " << child(i)->getMainCellVertexId() << "\n";
            std::cerr << "\t\t" << *(child(i)->getMainCell()) << "\n";
        }
        exit(0);
    }

    if( _geoLink != NULL )
    {
        delete _geoLink;
        _geoLink = NULL;
    }

    // Unregister the cells
    Require( nConnectedCells() ==0 );
    while( nConnectedCells() !=0 )
    {
        this->unregisterCell(this->connectedCell(0));
    }

    // Unregister this vertex from our parents childs
    // This is done in the destructor of GeoLink
    /*
    for( unsigned short i=0 ; i<this->_geoLink->getNGeoParents() ; ++i )
    {
        std::cerr << "For our parent " << i << "\n";
        if( this->_geoLink->getGeoParent(i)->isChild(this) )
        {
            std::cerr << "Removing us from our parent's children list\n";
            this->_geoLink->getGeoParent(i)->removeChild(this);
        }
        std::cerr << "For our parent " << i << " End\n";
    }
    */


}


void ConstrainedVertex::copyAllData( const ConstrainedVertex & cmsv )
{

    for( unsigned short i = 0 ; i<3 ; ++i )
        _positionROPointer[i] = cmsv._positionROPointer[i];

    _connectedCells = cmsv._connectedCells;

    _position = cmsv._position;
    _childs = cmsv._childs;
    _data = cmsv._data;
    _geoLink = new GeoLink( *cmsv._geoLink );
    _fatherCell = cmsv._fatherCell;
    _frame = cmsv._frame;
    _vertexId = cmsv._vertexId;
    _mainCell = cmsv._mainCell;

    std::cerr << "WARNING : copy All Data :\n";
    if( _mainCell != NULL )
        std::cerr << "\tmaincell is " << *_mainCell << "\n";
    getchar();
}


/*****************
 * End of constructors / desctructor
 */

bool ConstrainedVertex::isFree() const
{
    if( _geoLink == NULL )
        // ROOT VERTEX
        return true;
    else
        return _geoLink->isFree();
}

Vec3d ConstrainedVertex::get_Vec3d() const
{
    return _position;
}


ConstrainedVertex::operator const FloatingPointType*()
{
    Vec3d vec3d = this->get_Vec3d();

    _positionROPointer[0] = vec3d[0];
    _positionROPointer[1] = vec3d[1];
    _positionROPointer[2] = vec3d[2];

    return _positionROPointer;
}

FloatingPointType ConstrainedVertex::operator[]( int id ) const
{
    return this->get_Vec3d()[id];
}

FloatingPointType* ConstrainedVertex::get_FloatingPointTypePointerCopy() const
{
    Vec3d vec3d = this->get_Vec3d();

    FloatingPointType *toReturn = (FloatingPointType*) malloc( sizeof(FloatingPointType)*3 );
    toReturn[0] = vec3d[0];
    toReturn[1] = vec3d[1];
    toReturn[2] = vec3d[2];

    return toReturn;
}





// void ConstrainedVertex::set( FloatingPointType x, FloatingPointType y, FloatingPointType z, unsigned int depth ){
//  this->set( Vec3d(x,y,z), depth );
// }
// void ConstrainedVertex::set( Vec3d newPos, unsigned int depth )
// {
//  if( isFree() )
//  {
//      //updateChildrensPosition( newPos - _position, depth );
//      updateChildrensPositionOptimized( newPos - _position, depth );
//      clearDelta();
//      addDelta( newPos-_position );
//      _position = newPos;
//  }
//
//  else
//  {
//      updateChildrensPositionOptimizedNotFree( newPos - _position );
//  }
//
// }


typedef struct
{
    ConstrainedVertex *_vertex;
    Vec3d _vecToAdd;
}
UpdateChildrensPositionOptimizedData;

struct UpdateChildrensPositionOptimizedDataLess
{
    bool operator()( const UpdateChildrensPositionOptimizedData & d1,
                     const UpdateChildrensPositionOptimizedData &d2 ) const
    {
        return d1._vertex < d2._vertex;
    }
};

bool operator<( const UpdateChildrensPositionOptimizedData & d1,
                const UpdateChildrensPositionOptimizedData &d2 )
{
    return d1._vertex < d2._vertex;
}
bool operator!=( const UpdateChildrensPositionOptimizedData & d1,
                 const UpdateChildrensPositionOptimizedData &d2 )
{
    return d1._vertex != d2._vertex;
}


void ConstrainedVertex::updateChildrensPositionOptimized( Vec3d v, unsigned int depth )
{
    /*
     * Invariant : un element (unique par son pointeur) n'est qu'une seule fois dans le set
     */

    typedef std::map<ConstrainedVertex*,Vec3d> map;
    typedef std::deque<ConstrainedVertex*> list;

    map toAddMap;
    list vertexList;

    for( unsigned int i=0 ; i<_childs.size() ; i++ )
    {
        // Note : Could perhaps avoid some of this
        if( (_childs[i]->getDepth() > depth) || !_childs[i]->isFree() )
        {
            if( _childs[i]->getGeoLink()->isGeoLinkParent(this) )
            {
                _childs[i]->setDelta( Vec3d(0,0,0) );
                toAddMap[_childs[i]] = v / _childs[i]->getGeoLink()->getNGeoParents();
                vertexList.push_back( _childs[i] );
            }
        }
    }

    for( list::iterator it = vertexList.begin() ; it != vertexList.end() ; ++it )
    {
        (*it)->setDelta( Vec3d(0,0,0) );
    }

    while( !vertexList.empty() )
    {
        ConstrainedVertex* d = vertexList.front();
        vertexList.pop_front();

        for( unsigned i=0 ; i<d->_childs.size() ; ++i )
        {
            if( d->_childs[i]->getGeoLink()->isGeoLinkParent(d) )
            {
                // We need to add some Vec to this child
                Vec3d vecToAdd = toAddMap[d] / d->_childs[i]->getGeoLink()->getNGeoParents();

                // Now find it in the other list to see if we need to add it

                if( toAddMap.find(d->_childs[i]) == toAddMap.end() )
                {
                    // The child was not in the list before, so add it
                    //std::cerr << "Adding\n";
                    d->_childs[i]->setDelta( Vec3d(0,0,0) );
                    toAddMap[d->_childs[i]] = vecToAdd;
                    vertexList.push_back(d->_childs[i]);
                }
                else
                {
                    // Just add it
                    toAddMap[d->_childs[i]] += vecToAdd;
                }
            }
            else
            {
                // Perphaps no else !!!
            }
        }

        d->setPosition( d->_position + toAddMap[d] );

        d->setDelta( d->getDelta() + toAddMap[d] );
    }

}


// void ConstrainedVertex::addDelta( Vec3d d )
// {
//  _delta += d;
// }
//
// void ConstrainedVertex::clearDelta()
// {
//  _delta = Vec3d(0,0,0);
// }
//
// Vec3d ConstrainedVertex::getDelta() const
// {
//  return _delta;
// }

void ConstrainedVertex::updateChildrensPositionOptimizedNotFree( Vec3d v )
{
    /*
     * Invariant : un element (unique par son pointeur) n'est qu'une seule fois dans le set
     */

    Require( !isFree() );

    typedef std::map<ConstrainedVertex*,Vec3d> map;
    typedef std::deque<ConstrainedVertex*> list;

    map toAddMap;
    list vertexList;


    // Find the list of free parents
    list tmpParents;
    tmpParents.push_front( this );
    while( !tmpParents.empty() )
    {
        ConstrainedVertex* cour = tmpParents.front();
        Require( !cour->isFree() );

        tmpParents.pop_front();

        for( unsigned int i=0 ; i<cour->getGeoLink()->getNGeoParents() ; ++i )
        {
            ConstrainedVertex* parent = cour->getGeoLink()->getGeoParent(cour->getGeoLink()->getGeoParentId(i));
            if( !parent->isFree() )
            {
                tmpParents.push_back( parent );
            }
            else
            {
                // Ok, we can add it to the vertexList and map;
                // Only if it's not already in the map
                if( toAddMap.find(parent) == toAddMap.end() )
                {
                    toAddMap[ parent ] = v;
                    vertexList.push_back( parent );
                }
            }
        }
    }

    for( list::iterator it = vertexList.begin() ; it != vertexList.end() ; ++it )
    {
        (*it)->setDelta( Vec3d(0,0,0) );
    }
    /*
    for( unsigned int i=0 ; i<_childs.size() ; i++ )
    {
        // Note : Could perhaps avoid some of this
        if( (_childs[i]->getDepth() > depth) || !_childs[i]->isFree() )
        {
            if( _childs[i]->getGeoLink()->isGeoLinkParent(this) )
            {
                toAddMap[_childs[i]] = v / _childs[i]->getGeoLink()->getNGeoParents();
                vertexList.push_back( _childs[i] );
            }
        }
    }
    */

    while( !vertexList.empty() )
    {
        ConstrainedVertex* d = vertexList.front();
        vertexList.pop_front();

        for( unsigned i=0 ; i<d->_childs.size() ; ++i )
        {
            if( d->_childs[i]->getGeoLink()->isGeoLinkParent(d) )
            {
                // We need to add some Vec to this child
                Vec3d vecToAdd = toAddMap[d] / d->_childs[i]->getGeoLink()->getNGeoParents();

                // Now find it in the other list to see if we need to add it

                if( toAddMap.find(d->_childs[i]) == toAddMap.end() )
                {
                    // The child was not in the list before, so add it
                    //std::cerr << "Adding\n";
                    d->_childs[i]->setDelta( Vec3d(0,0,0) );
                    toAddMap[d->_childs[i]] = vecToAdd;
                    vertexList.push_back(d->_childs[i]);
                }
                else
                {
                    // Just add it
                    toAddMap[d->_childs[i]] += vecToAdd;
                }
            }
            else
            {
                // Perphaps no else !!!
            }
        }

        d->_position += toAddMap[d];
        d->setDelta( d->getDelta() + toAddMap[d] );
    }

}





/*
*
 * Update the position of our children
 * This is called for example when we are moving the vertex
 * We will then have to tell our children that we have moved
 */
void ConstrainedVertex::updateChildrensPosition( Vec3d v, unsigned int depth )
{
    for( unsigned int i=0 ; i<_childs.size() ; i++ )
    {
        if( _childs[i]->getDepth() > depth )
        {
            _childs[i]->updatePosition( this, v );
        }
        else if( !_childs[i]->isFree() )
        {
            // TODO !!
            _childs[i]->updateConstrainedChildrensPosition( v, depth );
        }
    }
}

void ConstrainedVertex::updateConstrainedChildrensPosition( Vec3d v, unsigned int depth )
{
    depth = 0;
    exit(0);
    Require( !isFree() );

    Vec3d newPos = _position + v / this->getGeoLink()->getNGeoParents();
    for( unsigned int i=0 ; i<_childs.size() ; i++ )
    {
        if( !_childs[i]->isFree() )
            // depth 0 because this vertex can never have children of depth superior
            updateConstrainedChildrensPosition( newPos - _position, 0 );

    }
    _position = newPos;
}




void ConstrainedVertex::updatePosition( ConstrainedVertex *parent, Vec3d v )
{
    Require( _geoLink != NULL );
    if( this->getGeoLink()->isGeoLinkParent(parent) )
    {
        Vec3d newPos = _position + v / this->getGeoLink()->getNGeoParents();
        // depth 0 because this vertex can never have children of depth superior
        updateChildrensPosition( newPos - _position, 0 );
        _position = newPos;
    }

}




ConstrainedVertex::operator const Vec3d() const
{
    return get_Vec3d();
}

void ConstrainedVertex::freeIt()
{
    Require( !isFree() );
    Require( _geoLink != NULL );

    this->getGeoLink()->freeIt();

    Ensure( isFree() );
    //updateFrame();
}


/*
 * Only a geo link for MR edition
 */
void ConstrainedVertex::softLinkIt( ConstrainedVertex* cv1, ConstrainedVertex* cv2 )
{
    Require( _geoLink != NULL );

    this->getGeoLink()->softLinkIt( cv1, cv2 );
    //  std::cerr << "\tLink position is : " << this->getGeoLink()->getPosition() << "\n";
    setValue( POS, getPosition() );
    //cerr<<"ConstrainedVertex::softLinkIt, init POS to: "<< value(POS) << endl;
    computeValueFromRelatives( VEL );

}
void ConstrainedVertex::softLinkIt( ConstrainedVertex* cv1, ConstrainedVertex* cv2, ConstrainedVertex* cv3, ConstrainedVertex* cv4 )
{
    Require( _geoLink != NULL );

    this->getGeoLink()->softLinkIt( cv1, cv2, cv3, cv4 );
    //updateFrame();
    setValue( POS, getPosition() );
    //cerr<<"ConstrainedVertex::softLinkIt, init POS to: "<< value(POS) << endl;
    computeValueFromRelatives( VEL );
}

void ConstrainedVertex::softLinkIt( ConstrainedVertex* cv1, ConstrainedVertex* cv2, ConstrainedVertex* cv3, ConstrainedVertex* cv4, ConstrainedVertex* cv5, ConstrainedVertex* cv6, ConstrainedVertex* cv7, ConstrainedVertex* cv8 )
{
    Require( _geoLink != NULL );

    this->getGeoLink()->softLinkIt( cv1, cv2, cv3, cv4, cv5, cv6, cv7, cv8 );
    //updateFrame();
    setValue( POS, getPosition() );
    //cerr<<"ConstrainedVertex::softLinkIt, init POS to: "<< value(POS) << endl;
    computeValueFromRelatives( VEL );
}


void ConstrainedVertex::hardLinkIt( ConstrainedVertex* cv1, ConstrainedVertex* cv2, ConstrainedVertex* cv3, ConstrainedVertex* cv4 )
{
    Require( isFree() );
    Require( _geoLink != NULL );

    this->getGeoLink()->hardLinkIt( cv1, cv2, cv3, cv4 );

    //set( this->getGeoLink()->getPosition() );

    Ensure( !isFree() );
    
    // on pourrait se limiter à la vitesse et la masse ?
    setValue( POS, getPosition() );
    computeValueFromParents( VEL );
    //cerr<<"ConstrainedVertex::hardLinkIt, init POS to: "<< value(POS) << endl;
//     for( int i=0; i<NB_AUXILIARY_VECTORS; ++i ){
//         cerr<<"ConstrainedVertex::hardLinkIt, value "<<i<<" = "<<value(i)<<endl;
//     }


}
void ConstrainedVertex::hardLinkIt( ConstrainedVertex* cv1, ConstrainedVertex* cv2 )
{
    Require( isFree() );
    Require( _geoLink != NULL );

    this->getGeoLink()->hardLinkIt( cv1, cv2 );

    //set( this->getGeoLink()->getPosition() );

    //updateFrame();

    Ensure( !isFree() );
    
    // on pourrait se limiter à la vitesse et la masse ?
    setValue( POS, getPosition() );
    //cerr<<"ConstrainedVertex::hardLinkIt, init POS to: "<< value(POS) << endl;
    computeValueFromParents( VEL );
/*    for( int i=0; i<NB_AUXILIARY_VECTORS; ++i ){
        cerr<<"ConstrainedVertex::hardLinkIt, value "<<i<<" = "<<value(i)<<endl;
    }*/
}



/*
void ConstrainedVertex::linkIt( ConstrainedVertex* cv1, ConstrainedVertex* cv2 ){
    Require( isFree() );
    Require( _geoLink != NULL );
    
    this->getGeoLink()->hardLinkIt( cv1, cv2 );
    set( this->getGeoLink()->getPosition() );
    
    Ensure( !isFree() );
}
*/
/*
void ConstrainedVertex::linkIt( ConstrainedVertex* cv1, ConstrainedVertex* cv2, ConstrainedVertex* cv3, ConstrainedVertex* cv4 ){
    Require( isFree() );
    Require( _geoLink != NULL );
    
    this->getGeoLink()->hardLinkIt( cv1, cv2, cv3, cv4 );
    set( this->getGeoLink()->getPosition() );
    
    Ensure( !isFree() );
}
*/

bool ConstrainedVertex::isChild( const ConstrainedVertex *cv ) const
{
    for( unsigned int i=0 ; i<_childs.size() ; i++ )
        if( _childs[i] == cv )
            return true; // Found

    return false;
}

void ConstrainedVertex::addChild( ConstrainedVertex *cv )
{
    Require( !isChild(cv) );
    //std::cerr << "addChild : cv is " << cv << " and this is " << this << "\n";
    //Require( cv->isParent(this) );

    _childs.push_back(cv);

    Ensure( isChild(cv) );
}
void ConstrainedVertex::removeChild( ConstrainedVertex *cv )
{
    Require( isChild(cv) );
    _childs.erase( _childs.begin() + getChildPos(cv) );
    Ensure( !isChild(cv) );
}

unsigned int ConstrainedVertex::getChildPos( ConstrainedVertex *cv ) const
{
    Require( isChild(cv) );
    unsigned int i;
    for( i=0 ; i<_childs.size() ; i++ )
        if( _childs[i] == cv )
            break;

    return i;
}


ConstrainedVertex * ConstrainedVertex::child( const unsigned int pos ) const
{
    Require( pos < nChildren() );
    return _childs[pos];
}

unsigned int ConstrainedVertex::nChildren() const
{
    return _childs.size();
}



bool ConstrainedVertex::isParent( const ConstrainedVertex *cv ) const
{
    std::cerr << "isParent() : cv is " << cv << " and this is " << this << "\n";
    return getGeoLink()->isParent(cv);
}





void ConstrainedVertex::registerCell( Cell *connectedCell )
{
    Require( !isConnected( connectedCell ) );
    //  Require( nConnectedCells() != 8 );

    //  _connectedCells[ nConnectedCells() ] = connectedCell;
    _connectedCells.push_back( connectedCell );

    Ensure( isConnected( connectedCell ) );
    //  Ensure( nConnectedCells() <= 8 );
}

void ConstrainedVertex::unregisterCell( const Cell* unconnectedCell )
{
    Require( isConnected( unconnectedCell ) );

    deque<Cell*>::iterator it;
    for( it = _connectedCells.begin() ; (it != _connectedCells.end()) && (*it != unconnectedCell) ; ++it )
        ;

    Require( it != _connectedCells.end() );
    _connectedCells.erase( it );

    /*
    unsigned short c;
    for( c=0 ; c<nConnectedCells() ; ++c )
        if( connectedCell( c ) == unconnectedCell )
            break;

    // Replace it by the last connectedCell
    _connectedCells[ c ] = connectedCell(nConnectedCells()-1);
    */

    Ensure( !isConnected( unconnectedCell ) );
}
unsigned short ConstrainedVertex::nConnectedCells() const
{
    //  Ensure( _connectedCells.size() <= 8 );
    /*
    if( _connectedCells.size() > 8 )
    {
        std::cerr << "Size is : " << _connectedCells.size() << "\n";
        for( unsigned int i=0 ; i<_connectedCells.size() ; ++i)
            std::cerr << "Cell : " << _connectedCells[i] << " ";
        std::cerr << "\n";
    }
    */
    return _connectedCells.size();
}
Cell* ConstrainedVertex::connectedCell( unsigned short id ) const
{
    Require( id < nConnectedCells() );

    return _connectedCells[id];
}
bool ConstrainedVertex::isConnected( const Cell* cell ) const
{
    unsigned short c;
    for( c=0 ; c<nConnectedCells() ; ++c )
        if( connectedCell( c ) == cell )
            break;
    if( c == nConnectedCells() )
        return false;
    else
        return true;
}


Cell* ConstrainedVertex::fatherCell() const
{
    return _fatherCell;
}

unsigned int ConstrainedVertex::getDepth() const
{
    /*
        if( fatherCell() == NULL )
            return 0;
        else
            return fatherCell()->getData()._depth+1;
        */
    return getMainCell()->getData()._depth;
}


void ConstrainedVertex::setData( const ConstrainedVertexData& d )
{
    _data = d;
}
ConstrainedVertexData ConstrainedVertex::getData( ) const
{
    return _data;
}


Vec3d ConstrainedVertex::getPosition() const
{
    return get_Vec3d();
}

GeoLink* ConstrainedVertex::getGeoLink() const
{
    Require( _geoLink != NULL );
    return _geoLink;
}


ConstrainedVertex*   ConstrainedVertex::getSmallestCellsFreeVertexSharingFaceForVertex( Cell *neighbour, unsigned int face, unsigned int vertex )
{
    //  Require( !neighbour->isLeaf() );

    unsigned int direction;
    if( face <= 1 )
        direction = 0; //x
    else if( face >= 4 )
        direction = 2; //z
    else
        direction = 1; //y

    Cell* res = neighbour;
    //  unsigned int childPos = Cell::connectedVertices[vertex][direction];

    if( !res->vertex(vertex)->isFree() )
    {
        do
        {
            res = res->father();
        }
        while( !res->vertex(vertex)->isFree() );
        Ensure( res->vertex(vertex)->isFree() );
        return res->vertex(vertex);
    }

    Cell *prec = NULL;

    while( !res->isLeaf() && res->vertex(vertex)->isFree() )
    {
        prec = res;
        res = res->child(vertex);
    }

    if( !res->vertex(vertex)->isFree() )
    {
        Require( prec != NULL );
        Ensure( prec->vertex(vertex)->isFree() );
        return prec->vertex(vertex);
    }
    else
    {
        Ensure( res->vertex(vertex)->isFree() );
        return res->vertex(vertex);
    }
}


/*
 * Not really free...
 * We will not get a free vertex if !res->vertex(connectedVertexId)->isFree()
 */
Cell*    ConstrainedVertex::getCellWithFreeVertexConnectedToVertex( Cell *cell, unsigned int direction, unsigned int vertexId ) const
{
    Require( direction < 3 );
    Require( vertexId < 8 );
    Require( cell != NULL );

    Cell* res = cell;
    unsigned int connectedVertexId = TempOctree::Cell::connectedVertices[vertexId][direction];

    if( !res->vertex(connectedVertexId)->isFree() )
    {
        return res;
        /*
        // Then go up in the hierarchy
        do
        {
            res = res->father();
        }
        while( !res->vertex(connectedVertexId)->isFree() );
        Ensure( res->vertex(connectedVertexId)->isFree() );
        return res;
        */
    }

    Cell *prec = NULL;

    while( !res->isLeaf() && res->vertex(connectedVertexId)->isFree() )
    {
        prec = res;
        res = res->child(vertexId);
    }

    if( !res->vertex(connectedVertexId)->isFree() )
    {
        Require( prec != NULL );
        Ensure( prec->vertex(connectedVertexId)->isFree() );
        return prec;
    }
    else
    {
        Ensure( res->vertex(connectedVertexId)->isFree() );
        return res;
    }
}


void ConstrainedVertex::updateFrame()
{
    _frame = computeFrame();
}


Frame ConstrainedVertex::computeFrame()
{
/*  ConstrainedVertex *_connectedVertices[2];
    FloatingPointType _connectedVerticesFactors[2];*/
    
    Require( hasMainCell() );
    
    Cell *neighbours[3];
    
    for( unsigned int i=0 ; i<3 ; ++i )
    {
        neighbours[i] = _mainCell->uncleSharingFace(TempOctree::verticesConnectedFaces[_vertexId][i]);
    }
    
    Vec3d vecs[3];

    Cell *cell = _mainCell ;
    
    while( !cell->isLeaf() )
    {
        cell = cell->child( this->getMainCellVertexId() );
    }
    
    cell = getVertexFreeCell(cell,this);
    
    
    for( unsigned short i=0 ; i<3 ; ++i )
    {
        neighbours[i] = cell->uncleSharingFace(TempOctree::verticesConnectedFaces[_vertexId][i]);
    
        Cell *connectedCell = getCellWithFreeVertexConnectedToVertex( cell, i, _vertexId );
    
        vecs[i] = connectedCell->vertex(Cell::connectedVertices[_vertexId][i])->getPosition() - this->getPosition();
        
        _connectedVertices[i][0] = connectedCell->vertex(Cell::connectedVertices[_vertexId][i]);
        _connectedVertices[i][1] = NULL;
        
        _connectedVerticesFactors[i][0] = 1.0;
        _connectedVerticesFactors[i][1] = 0.0;
        
        
    
        if( neighbours[i] == NULL )
        {
        // That's ok we've got it
        }
        else
        {
            vecs[i] = Vec3d(0,0,0);
            unsigned int dim = 0;
            
            vecs[i] += this->getPosition() - neighbours[i]->vertex(_vertexId)->getPosition();
            dim++;
        
            
            Cell *cFather = connectedCell;
        
            while( cFather->getData()._depth > neighbours[i]->getData()._depth )
            {
                cFather = cFather->father();
            }
        
            vecs[i] += cFather->vertex(Cell::connectedVertices[_vertexId][i])->getPosition() - this->getPosition();
            dim++;
        
            
            vecs[i] /= dim;
            
            _connectedVertices[i][0] = cFather->vertex(Cell::connectedVertices[_vertexId][i]);
            _connectedVertices[i][1] = neighbours[i]->vertex(_vertexId);
        
            _connectedVerticesFactors[i][0] = 0.5;
            _connectedVerticesFactors[i][1] = -0.5;
        }
    
    }
    
    
    if( TempOctree::hasBrotherAlongFace[_vertexId][0] )
    {
        vecs[0] *= -1;
        _connectedVerticesFactors[0][0] *= -1.0;
        _connectedVerticesFactors[0][1] *= -1.0;
    }
    if( TempOctree::hasBrotherAlongFace[_vertexId][2] )
    {
        vecs[1] *= -1;
        _connectedVerticesFactors[1][0] *= -1.0;
        _connectedVerticesFactors[1][1] *= -1.0;
    }
    if( TempOctree::hasBrotherAlongFace[_vertexId][4] )
    {
        vecs[2] *= -1;
        _connectedVerticesFactors[2][0] *= -1.0;
        _connectedVerticesFactors[2][1] *= -1.0;
    }
    

    return Frame(this->getPosition(),vecs[0],vecs[1],vecs[2]);
}

//
// return the smallest depth not constrained
//
unsigned int ConstrainedVertex::getConstrainedDepth() const
{
    int maxNCells = 0;
    
    Cell *neighbours[3];
    neighbours[0] = this->getMainCell()->getNeighbour( TempOctree::verticesConnectedFaces[_vertexId][0] );
    neighbours[1] = this->getMainCell()->getNeighbour( TempOctree::verticesConnectedFaces[_vertexId][1] );
    neighbours[2] = this->getMainCell()->getNeighbour( TempOctree::verticesConnectedFaces[_vertexId][2] );
    
    if( neighbours[0] )
        maxNCells++;
    if( neighbours[1] )
        maxNCells++;
    if( neighbours[2] )
        maxNCells++;
    
    if( maxNCells == 0 )
    {
        maxNCells = 1;
    }
    else
    {
        maxNCells = 1 << maxNCells;
    }
    
    // 1, 2, 4 or 8 cells of the same depth must meet !
    // => maxNCells
    
//  std::cerr << "Need to find " << maxNCells << " cells for " << *this->getMainCell() << " and ID is " << _vertexId << "\n";
    
    // on depth, give the number of cells
    std::map< int, int> nCellsOfDepth;
    for( unsigned int i=0 ; i<this->nConnectedCells() ; ++i )
    {
        nCellsOfDepth[this->connectedCell(i)->getData()._depth] += 1;
    }
    
    int deepest = -1;
    for( std::map< int, int>::iterator it=nCellsOfDepth.begin() ;
        it != nCellsOfDepth.end() ; ++it )
    {
//      std::cerr << "\tFor depth " << it->first << " Got " << it->second << " cells\n";
//      std::cerr << "Deepest is " << deepest << " and res " << (it->first > deepest) << "\n";
//      std::cerr << "maxNCells : " << maxNCells << " and res " << (it->second == maxNCells) << "\n";
        if( (it->second == maxNCells) && (it->first > deepest) )
        {
//          std::cerr << "\t\tGot deeper depth\n";
            deepest = it->first;
        }
    }
    
    Require( deepest != -1 );
    if( deepest == -1 )
    {
        std::cerr << "Weup deepest !\n";
    }
    
    return deepest;
}

// Frame ConstrainedVertex::computeFrame() const
// {
//  Require( isFree() );
//  
//  Vec3d pos = this->getPosition();
//  
//  Vec3d vecs[3];
//  
//  Cell *cell = getMainCell();
//  while( !cell->isLeaf() )
//  {
//      cell = cell->child( this->getMainCellVertexId() );
//  }
//  
//  for( unsigned int dir=0 ; dir<3 ; ++dir )
//  {
//      unsigned int nVecs = 0;
//      
//      Vec3d localVec;     unsigned int localDepth=INFINITY;   bool localOk = false;
//      Vec3d neighbourVec; unsigned int neighbourDepth=INFINITY;   bool neighbourOk = false;
//      FloatingPointType localFactor = 1.0;
//      FloatingPointType neighbourFactor = 1.0;
//      
//      unsigned int connectedId = Cell::connectedVertices[_vertexId][dir];
//      unsigned int face = Octree::verticesConnectedFaces[_vertexId][dir];
//      
//      // First find the vertex connected and free in cell or a parent cell
//      Cell *parentCell = cell;
//      
//      while( !parentCell->vertex(connectedId)->isFree() && (parentCell->vertex(connectedId)->getDepth()>this->getDepth()) )
//      {
//          parentCell = parentCell->father();
//      }
//      
//      localVec = parentCell->vertex( connectedId )->getPosition() - pos;
//      localDepth = parentCell->getData()._depth;
//      localOk = true;
//      
//      Cell *neighbour = cell->getNeighbour(face);
//      
//      // Can be something else
//      if( neighbour )
//      {
//          int diffDepth = 0;
//          while( !neighbour->isLeaf() )
//          {
//              neighbour = neighbour->child( connectedId );
//              diffDepth++;
//          }
//          
//          Cell *nParentCell = neighbour;
//          // First find the vertex connected and free in cell or a parent cell
//          while( !nParentCell->vertex(_vertexId)->isFree() && (nParentCell->vertex(_vertexId)->getDepth()>this->getDepth()) )
//          {
//              nParentCell = nParentCell->father();
//              diffDepth--;
//          }
//          
//          Require( diffDepth >= 0 );
//          
//          neighbourVec = nParentCell->vertex( _vertexId )->getPosition() - pos;
//          neighbourDepth = nParentCell->getData()._depth;
//          neighbourOk = true;
//          
//      }
//      
//      unsigned int smallestDepth = fmin(localDepth,neighbourDepth);
//      
//      if( localOk )
//      {
//          if( localDepth > smallestDepth )
//          {
//              localFactor = pow(2.0,localDepth-smallestDepth);
//          }
//          vecs[dir] += localFactor*localVec;
//      }
//      if( neighbourOk )
//      {
//          if( neighbourFactor > smallestDepth )
//          {
//              neighbourFactor = pow(2.0,neighbourFactor-smallestDepth);
//          }
//          vecs[dir] -= neighbourFactor*neighbourVec;
//      }
//      if( localOk && neighbourOk )
//      {
//          vecs[dir] /= 2;
//      }
//  }
//  
//  if( TempOctree::hasBrotherAlongFace[_vertexId][0] )
//             vecs[0] *= -1;
//         if( TempOctree::hasBrotherAlongFace[_vertexId][2] )
//             vecs[1] *= -1;
//         if( TempOctree::hasBrotherAlongFace[_vertexId][4] )
//             vecs[2] *= -1;
//      
//  return Frame( this->getPosition(), vecs[0], vecs[1], vecs[2] );
// }



// Frame ConstrainedVertex::computeFrame() const
// {
//  
//     Require( hasMainCell() );
// 
//     Cell *neighbours[3];
// 
//     for( unsigned int i=0 ; i<3 ; ++i )
//     {
//         neighbours[i] = _mainCell->uncleSharingFace(TempOctree::verticesConnectedFaces[_vertexId][i]);
//     }
// 
//     FloatingPointType constrained = !this->isFree();
//     Vec3d vecs[3];
// 
// 
// 
//     //else
//     if( !constrained )
//     {
//         //std::cerr << "computeFrame : " << *_mainCell << " Id is " << _vertexId << "\n";
//         //Require( _mainCell == getVertexFreeCell(_mainCell,this) );
// 
//         Cell *cell = _mainCell ;
// 
//         while( !cell->isLeaf() )
//         {
//             cell = cell->child( this->getMainCellVertexId() );
//         }
// 
//         cell = getVertexFreeCell(cell,this);
// 
// 
//         for( unsigned short i=0 ; i<3 ; ++i )
//         {
//             neighbours[i] = cell->uncleSharingFace(TempOctree::verticesConnectedFaces[_vertexId][i]);
// 
//             Cell *connectedCell = getCellWithFreeVertexConnectedToVertex( cell, i, _vertexId );
// 
//             vecs[i] = connectedCell->vertex(Cell::connectedVertices[_vertexId][i])->getPosition() - this->getPosition();
// 
//             if( neighbours[i] == NULL )
//             {
//                 // That's ok we've got it
//             }
//             else
//             {
//          vecs[i] = Vec3d(0,0,0);
//      unsigned int dim = 0;
//      
// //       if( this->getMainCell()->getData()._points.size() != 0 )
// //       {
//                  vecs[i] += this->getPosition() - neighbours[i]->vertex(_vertexId)->getPosition();
//          dim++;
// //       }
//      
//                 Cell *cFather = connectedCell;
// 
//                 while( cFather->getData()._depth > neighbours[i]->getData()._depth )
//                 {
//                     cFather = cFather->father();
//                 }
//      
// //       if( neighbours[i]->getData()._points.size() != 0 )
// //       {
//                  vecs[i] += cFather->vertex(Cell::connectedVertices[_vertexId][i])->getPosition() - this->getPosition();
//          dim++;
// //       }
//      
// //       Require( dim );
//      vecs[i] /= dim;
//             }
// 
// 
//             // WARNING
//             // Perhaps we should find the smallest unconstrained vertex here too
//             //Vec3d v1 = getSmallestCellsFreeVertexSharingFaceForVertex( getMainCell(), TempOctree::verticesConnectedFaces[_vertexId][i], _vertexId );
//             //           vecs[i] = _mainCell->vertex(Cell::connectedVertices[_vertexId][i])->getPosition() - this->getPosition();
// 
// 
// 
//         }
// 
// 
//         if( TempOctree::hasBrotherAlongFace[_vertexId][0] )
//             vecs[0] *= -1;
//         if( TempOctree::hasBrotherAlongFace[_vertexId][2] )
//             vecs[1] *= -1;
//         if( TempOctree::hasBrotherAlongFace[_vertexId][4] )
//             vecs[2] *= -1;
// 
// 
//     }
// 
//     //std::cerr << "Finaly le retour " << vecs[2] << "\n";
// 
// 
// //     if( vecs[0] != Vec3d(0.0,0.0,0.0) )
// //         vecs[0] /= vecs[0].norm();
// //     if( vecs[1] != Vec3d(0.0,0.0,0.0) )
// //         vecs[1] /= vecs[1].norm();
// //     if( vecs[2] != Vec3d(0.0,0.0,0.0) )
// //         vecs[2] /= vecs[2].norm();
// 
// 
//     //return Frame(vecs[0],vecs[1],vecs[2],true);
// 
// 
// // vecs[0] *= _mainCell->getData()._initialSize[0];
// // vecs[1] *= _mainCell->getData()._initialSize[1];
// // vecs[2] *= _mainCell->getData()._initialSize[2];
// 
// 
//     return Frame(this->getPosition(),vecs[0],vecs[1],vecs[2]);
// }


Frame & ConstrainedVertex::getFrame()
{
    return _frame;
}

bool ConstrainedVertex::hasMainCell( ) const
{
    return _mainCell != NULL;
}
void ConstrainedVertex::setMainCell( Cell* mainCell, unsigned int vId )
{
    //Require( !hasMainCell() );

    //std::cerr << "setMainCell\n";
    // Now update the geoLink stuff !!!
    if( (_geoLink != NULL) && (_mainCell != NULL) && (mainCell->father() != _mainCell->father()) )
    {
        //std::cerr << "\tChangeMainCell\n";
        _geoLink->changeCell( mainCell );
    }

    _mainCell = mainCell;
    _vertexId = vId;

}



Vec3d ConstrainedVertex::getParameters( Cell *cStart, unsigned short vId )
{
    Cell *curCell = cStart;
    ConstrainedVertex *curCV = curCell->vertex(vId);

    Vec3d params;
    unsigned int diffDepth = 0;
    while( !curCV->isFree() )
    {
        if( (curCell->fatherPos()%2) != 0 )
            params[0] += 1.0;
        if( (curCell->fatherPos()%4) >= 2 )
            params[1] += 1.0;
        if( curCell->fatherPos() >= 4 )
            params[2] += 1.0;

        params /= 2.0;

        curCell = curCell->father();
        curCV = curCell->vertex(vId);
        diffDepth++;
    }

    FloatingPointType pos = 1.0;
    if( diffDepth != 0 )
    {
        pos /= (FloatingPointType)(2<<(diffDepth-1));
    }

    if( (vId%2) != 0 )
        params[0] += pos;
    if( (vId%4) >= 2 )
        params[1] += pos;
    if( vId >= 4 )
        params[2] += pos;

    return params;
}

ConstrainedVertex* ConstrainedVertex::getFathersFreeVertex( Cell *cStart, unsigned short vId )
{
    Cell *curCell = cStart;
    ConstrainedVertex *curCV = curCell->vertex(vId);

    while( !curCV->isFree() )
    {
        curCell = curCell->father();
        curCV = curCell->vertex(vId);
    }

    Ensure( curCV->isFree() )   ;

    return curCV;
}

Cell* ConstrainedVertex::getFathersFreeVertexCell( Cell *cStart, unsigned short vId )
{
    /*
    Cell *curCell = cStart;
    ConstrainedVertex *curCV = curCell->vertex(vId);

    while( !curCV->isFree() )
    {
        curCell = curCell->father();
        curCV = curCell->vertex(vId);
    }

    Ensure( curCV->isFree() )   ;

    return curCell;
    */
    return getFathersFreeVertex( cStart, vId )->getMainCell();
}



Vec3d ConstrainedVertex::getParameters( Cell *cStart, unsigned int depth, unsigned short vId )
{
    unsigned int curDepth = depth;

    Cell *curCell = cStart;
    ConstrainedVertex *curCV = curCell->vertex(vId);

    Vec3d params;
    unsigned int diffDepth = 0;
    while( curDepth-- > 0 )
    {
        if( (curCell->fatherPos()%2) != 0 )
            params[0] += 1.0;
        if( (curCell->fatherPos()%4) >= 2 )
            params[1] += 1.0;
        if( curCell->fatherPos() >= 4 )
            params[2] += 1.0;

        params /= 2.0;

        curCell = curCell->father();
        curCV = curCell->vertex(vId);
        diffDepth++;
    }

    FloatingPointType pos = 1.0;
    if( diffDepth != 0 )
    {
        pos /= (FloatingPointType)(2<<(diffDepth-1));
    }

    if( (vId%2) != 0 )
        params[0] += pos;
    if( (vId%4) >= 2 )
        params[1] += pos;
    if( vId >= 4 )
        params[2] += pos;

    return params;
}




/*
 * Needed to get the alpha, beta and gamma coefficients for a CVertex
 */


FloatingPointType ConstrainedVertex::getAlpha( Cell *cell, unsigned int faceId, unsigned int vId, unsigned int diffDepth )
{
    FloatingPointType alpha = 0.0;

    std::deque<unsigned int> alphaFactors = getAlphaFactors( cell, faceId, diffDepth );
    Require( !alphaFactors.empty() );

    unsigned int denom = 2<<(alphaFactors.size()-1);
    unsigned int num = 0;

    // Calculate alpha
    for( unsigned int i=0 ; i<alphaFactors.size() ; ++i )
    {
        num += alphaFactors[i] * (denom>>(i+1));
    }
    alpha = (FloatingPointType)num / (FloatingPointType)denom;

    // Now add some 1/2^i if the vertex is at the top of the cell
    if( (vId%2) == 1 )
    {
        alpha += 1.0 / (FloatingPointType)denom;
    }

    return alpha;
}

FloatingPointType ConstrainedVertex::getBeta( Cell *cell, unsigned int faceId, unsigned int vId, unsigned int diffDepth )
{
    FloatingPointType beta = 0.0;

    std::deque<unsigned int> betaFactors = getBetaFactors( cell, faceId, diffDepth );
    Require( !betaFactors.empty() );

    unsigned int denom = 2<<(betaFactors.size()-1);
    unsigned int num = 0;

    // Calculate beta
    for( unsigned int i=0 ; i<betaFactors.size() ; ++i )
    {
        num += betaFactors[i] * (denom>>(i+1));
    }
    beta = (FloatingPointType)num / (FloatingPointType)denom;

    // Now add some 1/2^i if the vertex is at the top of the cell
    if( !((vId%4) <= 1) )
    {
        beta += 1.0 / (FloatingPointType)denom;
    }

    return beta;
}

FloatingPointType ConstrainedVertex::getGamma( Cell *cell, unsigned int faceId, unsigned int vId, unsigned int diffDepth )
{
    FloatingPointType gamma = 0.0;

    std::deque<unsigned int> gammaFactors = getGammaFactors( cell, faceId, diffDepth );
    Require( !gammaFactors.empty() );

    unsigned int denom = 2<<(gammaFactors.size()-1);
    unsigned int num = 0;

    // Calculate beta
    for( unsigned int i=0 ; i<gammaFactors.size() ; ++i )
    {
        num += gammaFactors[i] * (denom>>(i+1));
    }
    gamma = (FloatingPointType)num / (FloatingPointType)denom;

    // Now add some 1/2^i if the vertex is at the top of the cell
    if( vId >= 4 )
    {
        gamma += 1.0 / (FloatingPointType)denom;
    }

    return gamma;
}

std::deque<unsigned int> ConstrainedVertex::getAlphaFactors( Cell *cell, unsigned int faceId, unsigned int depthLeft )
{
    //std::cerr << "Faceid is " << faceId << " and fatherpos is " << cell->fatherPos() << "\n";
    Require( !cell->isRoot() );

    if( depthLeft == 0 )
    {
        return std::deque<unsigned int>(0);
    }
    else
    {
        std::deque<unsigned int> parents = getAlphaFactors( cell->father(), faceId, depthLeft-1 );

        unsigned int myId = cell->fatherPos();
        if( (myId%2) == 0 )
            // This is a bottom cell, return 0
            parents.push_back(0);
        else
            parents.push_back(1);

        return parents;
    }
}

std::deque<unsigned int> ConstrainedVertex::getBetaFactors( Cell *cell, unsigned int faceId, unsigned int depthLeft )
{
    //std::cerr << "Faceid is " << faceId << " and fatherpos is " << cell->fatherPos() << "\n";
    Require( !cell->isRoot() );

    if( depthLeft == 0 )
    {
        return std::deque<unsigned int>(0);
    }
    else
    {
        std::deque<unsigned int> parents = getBetaFactors( cell->father(), faceId, depthLeft-1 );

        unsigned int myId = cell->fatherPos();
        if( (myId%4) <= 1 )
            // This is a bottom cell, return 0
            parents.push_back(0);
        else
            parents.push_back(1);

        return parents;
    }
}

std::deque<unsigned int> ConstrainedVertex::getGammaFactors( Cell *cell, unsigned int faceId, unsigned int depthLeft )
{
    if( depthLeft == 0 )
    {
        return std::deque<unsigned int>(0);
    }
    else
    {
        std::deque<unsigned int> parents = getGammaFactors( cell->father(), faceId, depthLeft-1 );

        unsigned int myId = cell->fatherPos();
        if( myId <= 3 )
            // This is a bottom cell, return 0
            parents.push_back(0);
        else
            parents.push_back(1);

        return parents;
    }
}


Cell* ConstrainedVertex::getMainCell() const
{
    return _mainCell;
}




std::set
    <ConstrainedVertex*> ConstrainedVertex::getFreeParentVertices( Cell *cell )
{
    std::list<ConstrainedVertex*> queueVertices;
    std::set
        <ConstrainedVertex*> freeVertices, seenVertices;

    for( unsigned short vId=0 ; vId<8 ; ++vId )
    {
        queueVertices.push_back( cell->vertex(vId) );
        //std::cerr << "In this cell : " << cell->vertex(vId) << "\n";
    }

    // Add to the set all the freeVertices parents of the vertices
    // in queueVertices (if any)
    while( !queueVertices.empty() )
    {
        ConstrainedVertex *cv = queueVertices.front();
        queueVertices.pop_front();

        //std::cerr << "Removing " << cv << "\n";

        seenVertices.insert( cv );

        if( cv->isFree() )
        {
            freeVertices.insert(cv);
        }
        else
        {
            for( unsigned short i=0 ; i<cv->getGeoLink()->getNGeoParents() ; ++i )
            {
                ConstrainedVertex *parent = cv->getGeoLink()->getGeoParent( cv->getGeoLink()->getGeoParentId(i) );
                if( seenVertices.find(parent) == seenVertices.end() )
                {
                    queueVertices.push_back(parent);
                }
            }
        }
    }

    return freeVertices;
}




// void ConstrainedVertex::moveIndependent( FloatingPointType x, FloatingPointType y, FloatingPointType z )
// {
//  moveIndependent( Vec3d(x,y,z) );
// }
//
// void ConstrainedVertex::moveIndependent( Vec3d newPos )
// {
//  Require( isFree() );
//
//  this->_position = newPos;
//
//  std::list<ConstrainedVertex*> queue;
//
//  for( unsigned int i=0 ; i<this->nChildren() ; ++i )
//  {
//      if( ! this->child(i)->isFree() )
//      {
//          queue.push_back( this->child(i) );
//      }
//  }
//
//  for( std::list<ConstrainedVertex*>::iterator it = queue.begin() ;
//      it != queue.end(); ++it )
//  {
//      Require( !(*it)->isFree() );
//
//      (*it)->_position = (*it)->computeConstrainedPosition();
//
//      for( unsigned int i=0 ; i<(*it)->nChildren() ; ++i )
//      {
//          if( ! (*it)->child(i)->isFree() )
//          {
//              queue.push_back( (*it)->child(i) );
//          }
//      }
//
//  }
//
// }

Vec3d ConstrainedVertex::computePosition() const
{
    Require( !isFree() );

    Vec3d res;

    for( unsigned short i=0 ; i<getGeoLink()->getNGeoParents() ; ++i )
    {
        res += getGeoLink()->getGeoParent( getGeoLink()->getGeoParentId(i) )->getPosition();
    }

    res /= getGeoLink()->getNGeoParents();

    return res;
}


void ConstrainedVertex::setPosition( Vec3d newPos )
{
    _position = newPos;
    this->getFrame().setOrigin( _position );
}

void ConstrainedVertex::setPositionAndPropagate( Vec3d newPos )
{
    Require( isFree() );

//     this->_position = newPos;
    this->setPosition( newPos );

    std::list<ConstrainedVertex*> queue;

    for( unsigned int i=0 ; i<this->nChildren() ; ++i )
    {
        if( ! this->child(i)->isFree() )
        {
            queue.push_back( this->child(i) );
        }
    }

    for( std::list<ConstrainedVertex*>::iterator it = queue.begin() ;
            it != queue.end(); ++it )
    {
        Require( !(*it)->isFree() );

        (*it)->setPosition( (*it)->computePosition() );

        for( unsigned int i=0 ; i<(*it)->nChildren() ; ++i )
        {
            if( ! (*it)->child(i)->isFree() )
            {
                queue.push_back( (*it)->child(i) );
            }
        }

    }
}

void ConstrainedVertex::setPositionAndPropagateAll( Vec3d newPos, unsigned int depth )
{
    if( isFree() )
    {
        //updateChildrensPosition( newPos - _position, depth );
        updateChildrensPositionOptimized( newPos - _position, depth );
        setDelta( Vec3d(0,0,0) );
        setDelta( getDelta() + newPos-_position );
        setPosition( newPos );
    }

    else
    {
        updateChildrensPositionOptimizedNotFree( newPos - _position );
    }
}




Vec3d ConstrainedVertex::getDelta() const
{
    return _delta;
}
void ConstrainedVertex::setDelta( Vec3d d )
{
    _delta = d;
}
Vec3d ConstrainedVertex::computeDelta() const
{
    Require( !isFree() );

    Vec3d res;
    unsigned short nParents = getGeoLink()->getNGeoParents();

    for( unsigned short i=0 ; i<nParents ; ++i )
    {
        res += getGeoLink()->getGeoParent( getGeoLink()->getGeoParentId(i) )->getDelta();
    }

    res /= nParents;

    return res;
}



// Same thing for auxiliary vectors

Vec3d ConstrainedVertex::computeValue(int value_id ) const
{
    Require( !isFree() );

    Vec3d res;

    for( unsigned short i=0 ; i<getGeoLink()->getNGeoParents() ; ++i )
    {
        res += getGeoLink()->getGeoParent( getGeoLink()->getGeoParentId(i) )->value( value_id );
    }

    res /= getGeoLink()->getNGeoParents();

    return res;
}

void ConstrainedVertex::computeValueFromParents(int value_id )
{
    Require( !isFree() );

    Vec3d res;

    for( unsigned short i=0 ; i<getGeoLink()->getNGeoParents() ; ++i )
    {
        res += getGeoLink()->getGeoParent( getGeoLink()->getGeoParentId(i) )->value( value_id );
    }

    res /= getGeoLink()->getNGeoParents();

    setValue(value_id, res);
}

void ConstrainedVertex::computeValueFromRelatives(int value_id )
{
    Require( isFree() );

    Vec3d res;

    for( unsigned short i=0 ; i<getGeoLink()->getNGeoParents() ; ++i )
    {
        res += getGeoLink()->getGeoParent( getGeoLink()->getGeoParentId(i) )->value( value_id );
    }

    res /= getGeoLink()->getNGeoParents();

    setValue(value_id, res);
}

void ConstrainedVertex::setValue( int value_id, Vec3d newPos )
{
    _aux[value_id] = newPos;
}

Vec3d& ConstrainedVertex::value( int value_id )
{
    return _aux[value_id];
}

const Vec3d& ConstrainedVertex::value( int value_id ) const
{
    return _aux[value_id];
}

void ConstrainedVertex::setValueAndPropagate( int value_id, Vec3d newPos )
{
    Require( isFree() );

    this->_aux[value_id] = newPos;

    std::list<ConstrainedVertex*> queue;

    for( unsigned int i=0 ; i<this->nChildren() ; ++i )
    {
        if( ! this->child(i)->isFree() )
        {
            queue.push_back( this->child(i) );
        }
    }

    for( std::list<ConstrainedVertex*>::iterator it = queue.begin() ;
            it != queue.end(); ++it )
    {
        Require( !(*it)->isFree() );

        (*it)->setValue( value_id, (*it)->computeValue(value_id) );

        for( unsigned int i=0 ; i<(*it)->nChildren() ; ++i )
        {
            if( ! (*it)->child(i)->isFree() )
            {
                queue.push_back( (*it)->child(i) );
            }
        }

    }
}












Cell::vertex_connected_iterator::vertex_connected_iterator( ConstrainedVertex* vertex, Cell *cell, unsigned int vPos )
{
    //  std::cerr << "Cell is " << *cell << "\n";

    for( unsigned short dir=0 ; dir<3 ; ++dir )
    {
        _vList.push_back( cell->vertex(connectedVertices[vPos][dir]) );

        Cell *neighbour = cell->uncleSharingFace(TempOctree::verticesConnectedFaces[vPos][dir]);
        if( (neighbour != NULL) && vertex->isConnected(neighbour) )
        {
            Cell *otherCell = vertex->getCellWithFreeVertexConnectedToVertex( neighbour, dir, vPos );
            //          std::cerr << "Adding vertex " << vPos << " from cell " << *otherCell << "\n";
            _vList.push_back( otherCell->vertex(vPos) );
        }
    }

    /*
    for( unsigned short i=0 ; i < vertex->nConnectedCells() ; ++i )
    {
        Cell *connectedCell = vertex->connectedCell( i );
        if( connectedCell->getData()._depth <= depth )
        {
            // Find the corresponding vertex in this new cell
            unsigned short child;
            for( child=0 ; child<8 ; ++child )
            {
                if( connectedCell->vertex(child) == vertex )
                {
                    for( unsigned short dir=0 ; dir<3 ; ++dir )
                    {
                        _vSet.insert( connectedCell->vertex( Cell::connectedVertices[child][dir] ) );
                    }
                    break;
                }
            }
            Require( child != 8 );
        }
    }
    */

}

Cell::vertex_connected_iterator::~vertex_connected_iterator()
{}

TempOctree::Vertex* Cell::vertex_connected_iterator::operator++()
{
    Require( !empty() );

    /*
    Vertex* res = *_vSet.begin();
    _vSet.erase( _vSet.begin() );
    */
    Vertex* res = _vList.front();
    _vList.pop_front();

    return res;
}
bool Cell::vertex_connected_iterator::empty()
{
    return _vList.empty();
    //return _vSet.empty();
}


}
}

---