int MultilevelHexahedronSetTopologyContainer::getHexaChildren(const unsigned int hexaId,
        helper::vector<unsigned int>& children) const
{
    std::list<Component*>	compList;
    compList.push_back(_coarseComponents.getValue()[hexaId]);

    Component* comp = compList.front();
    while(!comp->_children.empty())
    {
        for(std::set<Component*>::iterator iter = comp->_children.begin();
            iter != comp->_children.end(); ++iter)
        {
            compList.push_back(*iter);
        }

        compList.pop_front();
        comp = compList.front();
    }

    std::set<Component*> compSet;
    compSet.insert(compList.begin(), compList.end());

    children.reserve(compSet.size());

    for(unsigned int i=0; i<_fineComponents.getValue().size(); ++i)
    {
        if(compSet.find(_fineComponents.getValue()[i]) != compSet.end())
            children.push_back(i);
    }

    return (int) children.size();
}
Ejemplo n.º 2
0
void TimerData::clear()
{
    nbIter = 0;
    steps.clear();
    stepData.clear();
    vals.clear();
    valData.clear();
}
Ejemplo n.º 3
0
void MeshGmshLoader::addInGroup(helper::vector< sofa::core::loader::PrimitiveGroup>& group,int tag,int /*eid*/) {
    for (unsigned i=0;i<group.size();i++) {
        if (tag == group[i].p0) {
            group[i].nbp++;
            return;
        }
    }

    stringstream ss;
    string s;
    ss << tag;

    group.push_back(sofa::core::loader::PrimitiveGroup(tag,1,s,s,-1));
}
void ClosestPointRegistrationForceField<DataTypes>::detectBorder(vector<bool> &border,const helper::vector< tri > &triangles)
{
    unsigned int nbp=border.size();
    unsigned int nbt=triangles.size();
    for(unsigned int i=0;i<nbp;i++) border[i]=false;

    if(!nbt) return;
    vector<vector< unsigned int> > ngbTriangles((int)nbp);
    for(unsigned int i=0;i<nbt;i++) for(unsigned int j=0;j<3;j++)	ngbTriangles[triangles[i][j]].push_back(i);
    for(unsigned int i=0;i<nbp;i++) if(ngbTriangles[i].size()==0) border[i]=true;
    for(unsigned int i=0;i<nbt;i++)
        for(unsigned int j=0;j<3;j++)
        {
            unsigned int id1=triangles[i][j],id2=triangles[i][(j==2)?0:j+1];
            if(!border[id1] || !border[id2]) {
                bool bd=true;
                for(unsigned int i1=0;i1<ngbTriangles[id1].size() && bd;i1++)
                    for(unsigned int i2=0;i2<ngbTriangles[id2].size() && bd;i2++)
                        if(ngbTriangles[id1][i1]!=i)
                            if(ngbTriangles[id1][i1]==ngbTriangles[id2][i2])
                                bd=false;
                if(bd) border[id1]=border[id2]=true;
            }
        }
}
Ejemplo n.º 5
0
void MeshGmshLoader::normalizeGroup(helper::vector< sofa::core::loader::PrimitiveGroup>& group) {
    int start = 0;
    for (unsigned i=0;i<group.size();i++) {
        group[i].p0 = start;
        start += group[i].nbp;
    }
}
Ejemplo n.º 6
0
/**
 * Compute distances between both point clouds (symmetrical and non-symmetrical distances)
 */
SReal InertiaAlign::computeDistances( helper::vector<sofa::defaulttype::Vec<3,SReal> > S, helper::vector<sofa::defaulttype::Vec<3,SReal> > T)
{
    SReal maxST = 0.0;
    for (unsigned int i = 0 ; i < S.size(); i++)
    {
        SReal d = InertiaAlign::distance(S[i], T);
        if (d>maxST) maxST = d;
    }

    SReal maxTS = 0.0;
    for (unsigned int i = 0 ; i < T.size(); i++)
    {
        SReal d = InertiaAlign::distance(T[i], S);
        if (d>maxTS) maxTS = d;
    }

    if (maxTS > maxST)
        return maxST/S.size();
    else
        return maxTS/S.size();

}
Ejemplo n.º 7
0
void LMatrixManipulator::buildLMatrix(const helper::vector<LLineManipulator> &lines, SparseMatrixEigen& matrix) const
{
    for (unsigned int l=0; l<lines.size(); ++l)
    {
        const LLineManipulator& lManip=lines[l];
        SparseVectorEigen vector;
        lManip.buildCombination(LMatrix,vector);
        matrix.startVec(l);
        for (SparseVectorEigen::InnerIterator it(vector); it; ++it)
        {
            matrix.insertBack(l,it.index())=it.value();
        }
    }
}
Ejemplo n.º 8
0
void GraphModeler::saveComponents(helper::vector<QTreeWidgetItem*> items, const std::string &file)
{
    std::ofstream out(file.c_str());
    simulation::XMLPrintVisitor print(sofa::core::ExecParams::defaultInstance() /* PARAMS FIRST */, out);
    print.setLevel(1);
    out << "<Node name=\"Group\">\n";
    for (unsigned int i=0; i<items.size(); ++i)
    {
        if (BaseObject* object=getObject(items[i]))
            print.processBaseObject(object);
        else if (Node *node=getNode(items[i]))
            print.execute(node);
    }
    out << "</Node>\n";
}
    /// Generate discrete mass position values with variational sympletic implicit solver
    void generateDiscreteMassPositions (double h, double K, double m, double z0, double v0,double g, double finalTime, double rm,double rk)
    {
        int size = 0 ;

        // During t=finalTime
        if((finalTime/h) > 0)
        {
            size = int(finalTime/h);
            positionsArray.reserve(size);
            velocitiesArray.reserve(size);
            energiesArray.reserve(size);
        }

        // First velocity is v0
        velocitiesArray.push_back(v0);

        // First acceleration
        energiesArray.push_back(m*v0);

        // First position is z0
        positionsArray.push_back(double(z0));

        // Compute totalEnergy
        totalEnergy = m*g*z0; // energy at initial time

        // Set constants
        double denominator = 4*m+h*h*K+4*h*(rm*m + rk*K);//4*h*(-rk*K+rm*m);
        double constant = -h*K;

        // Compute velocities, energies and positions
        for(int i=1;i< size+1; i++)
        {
            velocitiesArray.push_back(2*(-m*g*h+constant*(positionsArray[i-1]-z0)+2*energiesArray[i-1])/denominator);
            energiesArray.push_back(m*velocitiesArray[i]+h*(-K*(positionsArray[i-1]- z0+velocitiesArray[i]*h/2) -m*g)/2);
            positionsArray.push_back(positionsArray[i-1]+h*velocitiesArray[i]);
        }

    }
Ejemplo n.º 10
0
void GlText::textureDraw_Indices(const helper::vector<defaulttype::Vector3>& positions, const double& scale)
{
    if (!s_asciiTexture)
    {
        GlText::initTexture();
        s_asciiTexture->init();
    }
    defaulttype::Mat<4, 4, GLfloat> modelviewM;

    const unsigned int nb_char_width = 16;
    const unsigned int nb_char_height = 16;
    const float worldHeight = 1.0;
    const float worldWidth = 0.5;

    glPushAttrib(GL_TEXTURE_BIT);
    glEnable(GL_TEXTURE_2D);

    glTexEnvf(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_MODULATE); // multiply tex color with glColor
    //glTexEnvf(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_REPLACE); // only tex color (no glColor)
    glEnable(GL_ALPHA_TEST);
    glAlphaFunc(GL_GREATER, 0.0);

    s_asciiTexture->bind();

    for (unsigned int i = 0; i < positions.size(); i++)
    {
        std::ostringstream oss;
        oss << i;
        std::string str = oss.str();
        unsigned int length = str.size();

        std::vector<Vector3> vertices;
        std::vector<Vector2> UVs;

        glDisable(GL_LIGHTING);

        glPushMatrix();

        // Makes text always face the viewer by removing the scene rotation
        // get the current modelview matrix
        glGetFloatv(GL_MODELVIEW_MATRIX, modelviewM.ptr());
        modelviewM.transpose();

        defaulttype::Vec3d temp(positions[i][0], positions[i][1], positions[i][2]);
        temp = modelviewM.transform(temp);

        glLoadIdentity();
        //translate a little bit to center the text on the position (instead of starting from a top-left position)
        glTranslatef((float)temp[0] - (worldWidth*length*scale)*0.5, (float)temp[1] + worldHeight*scale*0.5, (float)temp[2]);
        glScalef((float)scale, (float)scale, (float)scale);
        glRotatef(180.0, 1, 0, 0);
        for (unsigned int j = 0; j < length; j++)
        {
            Vector3 vertex_up_left = Vector3(j*worldWidth, worldHeight, 0.0f);
            Vector3 vertex_up_right = Vector3(j*worldWidth + worldWidth, worldHeight, 0.0f);
            Vector3 vertex_down_right = Vector3(j*worldWidth + worldWidth, 0.0f, 0.0f);
            Vector3 vertex_down_left = Vector3(j*worldWidth, 0.0f, 0.0f);

            vertices.push_back(vertex_up_left);
            vertices.push_back(vertex_down_left);
            vertices.push_back(vertex_down_right);
            vertices.push_back(vertex_up_right);

            char character = str[j] - 32;

            float uv_x = (character % nb_char_width) / (float)nb_char_width;
            float uv_y = 1.0f - ((character / nb_char_height) / (float)nb_char_height);

            Vector2 uv_up_left = Vector2(uv_x, (uv_y - (1.0f / (float)nb_char_height)));
            Vector2 uv_up_right = Vector2(uv_x + (1.0f / (float)nb_char_width), (uv_y - (1.0f / (float)nb_char_height)));
            Vector2 uv_down_right = Vector2(uv_x + (1.0f / (float)nb_char_width), uv_y);
            Vector2 uv_down_left = Vector2(uv_x, uv_y);

            UVs.push_back(uv_up_left);
            UVs.push_back(uv_down_left);
            UVs.push_back(uv_down_right);
            UVs.push_back(uv_up_right);
        }

        glBegin(GL_QUADS);
        for (unsigned int j = 0; j < vertices.size(); j++)
        {
            glTexCoord2fv(UVs[j].data());
            glVertex3fv(vertices[j].data());
        }
        glEnd();

        glPopMatrix();
    }

    s_asciiTexture->unbind();
    glDisable(GL_ALPHA_TEST);
    glPopAttrib();


    glEnable(GL_LIGHTING);
}
Ejemplo n.º 11
0
void TimerData::print()
{
    static ctime_t tmargin = CTime::getTicksPerSec() / 100000;
    std::ostream& out = std::cout;
    out << "==== " << id << " ====\n\n";
    if (!records.empty())
    {
        out << "Trace of last iteration :\n";
        ctime_t t0 = records[0].time;
        ctime_t last_t = 0;
        int level = 0;
        for (unsigned int ri = 1; ri < records.size(); ++ri)
        {
            const Record& r = records[ri];
            out << "  * ";
            if (ri > 0 && ri < records.size()-1 && r.time <= last_t + tmargin)
            {
                printNoVal(out);
                out << "   ";
            }
            else
            {
                printTime(out, r.time - t0);
                out << " ms";
                last_t = r.time;
            }
            out << " ";
            if (r.type == Record::REND || r.type == Record::RSTEP_END) --level;
            for (int l=0; l<level; ++l)
                out << "  ";
            switch(r.type)
            {
            case Record::RNONE:
                out << "NONE";
                break;
            case Record::RSTEP_BEGIN:
                out << "> begin " << AdvancedTimer::IdStep(r.id);
                if (r.obj)
                    out << " on " << AdvancedTimer::IdObj(r.obj);
                break;
            case Record::RSTEP_END:
                out << "< end   " << AdvancedTimer::IdStep(r.id);
                if (r.obj)
                    out << " on " << AdvancedTimer::IdObj(r.obj);
                break;
            case Record::RSTEP:
                out << "- step  " << AdvancedTimer::IdStep(r.id);
                if (r.obj)
                    out << " on " << AdvancedTimer::IdObj(r.obj);
                break;
            case Record::RVAL_SET:
                out << ": var   " << AdvancedTimer::IdVal(r.id);
                out << "  = " << r.val;
                break;
            case Record::RVAL_ADD:
                out << ": var   " << AdvancedTimer::IdVal(r.id);
                out << " += " << r.val;
                break;
            case Record::REND:
                out << "END";
                break;
            default:
                out << "UNKNOWN RECORD TYPE" << (int)r.type;
            }
            out << std::endl;
            if (r.type == Record::RBEGIN || r.type == Record::RSTEP_BEGIN) ++level;
        }
    }
    if (!steps.empty())
    {
        out << "\nSteps Duration Statistics (in ms) :\n";
        out << " LEVEL\t START\t  NUM\t   MIN\t   MAX\t MEAN\t  DEV\t TOTAL\tPERCENT\tID\n";
        ctime_t ttotal = stepData[AdvancedTimer::IdStep()].ttotal;
        for (unsigned int s=0; s<steps.size(); ++s)
        {
            StepData& data = stepData[steps[s]];
            printVal(out, data.level);
            out << '\t';
            printTime(out, data.tstart, data.numIt);
            out << '\t';
            printVal(out, data.num, (s == 0) ? 1 : nbIter);
            out << '\t';
            printTime(out, data.tmin);
            out << '\t';
            printTime(out, data.tmax);
            out << '\t';
            double mean = (double)data.ttotal / data.num;
            printTime(out, (ctime_t)mean);
            out << '\t';
            printTime(out, (ctime_t)(sqrt((double)data.ttotal2/data.num - mean*mean)));
            out << '\t';
            printTime(out, data.ttotal, (s == 0) ? 1 : nbIter);
            out << '\t';
            printVal(out, 100.0*data.ttotal / (double) ttotal);
            out << '\t';
            if (s == 0)
                out << "TOTAL";
            else
            {
                for(int ii=0; ii<data.level; ii++) out<<".";  // indentation to show the hierarchy level
                out << steps[s];
            }
            out << std::endl;
        }
    }
    if (!vals.empty())
    {
        out << "\nValues Statistics :\n";
        out << " NUM\t  MIN\t  MAX\t MEAN\t  DEV\t TOTAL\tID\n";
        for (unsigned int s=0; s<vals.size(); ++s)
        {
            ValData& data = valData[vals[s]];
            printVal(out, data.num, nbIter);
            out << '\t';
            printVal(out, data.vmin);
            out << '\t';
            printVal(out, data.vmax);
            out << '\t';
            double mean = data.vtotal / data.num;
            printVal(out, mean);
            out << '\t';
            printVal(out, sqrt(data.vtotal2/data.num - mean*mean) );
            out << '\t';
            printVal(out, data.vtotal, nbIter);
            out << '\t';
            out << vals[s];
            out << std::endl;
        }
    }
    out << "\n iteration : " << getCurRecords()->size();
    out << "\n==== END ====\n";
    out << std::endl;
}
Ejemplo n.º 12
0
void TimerData::process()
{
    if (records.empty()) return;
    ++nbIter;
    if (nbIter == 0) return; // do not keep stats on very first iteration

    ctime_t t0 = records[0].time;
    //ctime_t last_t = 0;
    int level = 0;
    for (unsigned int ri = 0; ri < records.size(); ++ri)
    {
        const Record& r = records[ri];
        ctime_t t = r.time - t0;
        //last_t = r.time;
        if (r.type == Record::REND || r.type == Record::RSTEP_END) --level;
        switch (r.type)
        {
        case Record::RNONE:
            break;
        case Record::RBEGIN:
        case Record::RSTEP_BEGIN:
        case Record::RSTEP:
        {
            AdvancedTimer::IdStep id;
            if (r.type != Record::RBEGIN) id = AdvancedTimer::IdStep(r.id);
            if (stepData.find(id) == stepData.end())
                steps.push_back(id);
            StepData& data = stepData[id];
            data.level = level;
            if (data.lastIt != nbIter)
            {
                data.lastIt = nbIter;
                data.tstart += t;
                ++data.numIt;
            }
            data.lastTime = t;
            ++data.num;
            break;
        }
        case Record::REND:
        case Record::RSTEP_END:
        {
            AdvancedTimer::IdStep id;
            if (r.type != Record::REND) id = AdvancedTimer::IdStep(r.id);
            StepData& data = stepData[id];
            if (data.lastIt == nbIter)
            {
                ctime_t dur = t - data.lastTime;
                data.ttotal += dur;
                data.ttotal2 += dur*dur;
                if (data.num == 1 || dur > data.tmax) data.tmax = dur;
                if (data.num == 1 || dur < data.tmin) data.tmin = dur;
            }
            break;
        }
        case Record::RVAL_SET:
        case Record::RVAL_ADD:
        {
            AdvancedTimer::IdVal id = AdvancedTimer::IdVal(r.id);
            if (valData.find(id) == valData.end())
                vals.push_back(id);
            ValData& data = valData[id];
            if (r.type == Record::RVAL_SET || (data.lastIt != nbIter))
            {
                // update vmin and vmax
                if (data.num == 1 || data.vtotalIt < data.vmin) data.vmin = data.vtotalIt;
                if (data.num == 1 || data.vtotalIt > data.vmax) data.vmax = data.vtotalIt;
            }
            if (data.lastIt != nbIter)
            {
                data.lastIt = nbIter;
                data.vtotalIt = r.val;
                data.vtotal += r.val;
                data.vtotal2 += r.val*r.val;
                ++data.numIt;
                ++data.num;
            }
            else if (r.type == Record::RVAL_SET)
            {
                data.vtotalIt = r.val;
                data.vtotal += r.val;
                data.vtotal2 += r.val*r.val;
                ++data.num;
            }
            else
            {
                data.vtotalIt += r.val;
                data.vtotal += r.val;
                data.vtotal2 += r.val*r.val;
            }
            break;
        }
        }

        if (r.type == Record::RBEGIN || r.type == Record::RSTEP_BEGIN) ++level;
    }

    for (unsigned int vi=0; vi < vals.size(); ++vi)
    {
        AdvancedTimer::IdVal id = vals[vi];
        ValData& data = valData[id];
        if (data.num > 0)
        {
            // update vmin and vmax
            if (data.num == 1 || data.vtotalIt < data.vmin) data.vmin = data.vtotalIt;
            if (data.num == 1 || data.vtotalIt > data.vmax) data.vmax = data.vtotalIt;
        }
    }
}
Ejemplo n.º 13
0
json TimerData::getLightJson(std::string stepNumber)
{
    json jsonOutput;
    std::vector<std::string> deepthTree;
    std::string jsonObjectName = stepNumber;
    std::string father;
    int componantLevel = 0;
    int subComponantLevel = 0;
    std::stringstream ComposantId;

    if (!steps.empty())
    {
        // Clean the streamString
        ComposantId.str("");
        componantLevel = 0;
        subComponantLevel = 0;

        // Create the JSON container
        jsonOutput[jsonObjectName];

        for (unsigned int s=0; s<steps.size(); s++)
        {
            // Clean the streamString
            ComposantId.str("");

            StepData& data = stepData[steps[s]];

            if (s == 0)
            {
                ComposantId << "TOTAL";
                deepthTree.push_back(ComposantId.str());
                subComponantLevel = 0;
                jsonOutput[jsonObjectName][ComposantId.str()]["Father"] = "None";
                jsonOutput[jsonObjectName][ComposantId.str()]["Values"] = createJSONArray(s, jsonOutput[jsonObjectName][ComposantId.str()]["Values"], data);
            }
            else
            {
                for(int ii=0; ii<data.level; ii++) ++subComponantLevel;  // indentation to show the hierarchy level

                // If the level increment
                if(componantLevel < subComponantLevel)
                {
                    father = deepthTree.at(deepthTree.size()-1);
                    ComposantId << steps[s];
                    deepthTree.push_back(ComposantId.str());
                    jsonOutput[jsonObjectName][ComposantId.str()]["Father"] = father;
                    jsonOutput[jsonObjectName][ComposantId.str()]["Values"] = createJSONArray(s, jsonOutput[jsonObjectName][ComposantId.str()]["Values"], data);;

                }
                // If the level decrement
                else if(componantLevel > subComponantLevel)
                {
                    deepthTree.pop_back();
                    father = deepthTree.at(deepthTree.size()-1);
                    ComposantId << steps[s];

                    jsonOutput[jsonObjectName][ComposantId.str()]["Father"] = father;
                    jsonOutput[jsonObjectName][ComposantId.str()]["Values"] = createJSONArray(s, jsonOutput[jsonObjectName][ComposantId.str()]["Values"], data);

                }

                // If the level stay the same
                else if (componantLevel == subComponantLevel)
                {
                    ComposantId << steps[s];

                    jsonOutput[jsonObjectName][ComposantId.str()]["Father"] = father;
                    jsonOutput[jsonObjectName][ComposantId.str()]["Values"] = createJSONArray(s, jsonOutput[jsonObjectName][ComposantId.str()]["Values"], data);
                }

            }

            componantLevel = subComponantLevel;
            subComponantLevel = 0;


        }
    }

    return jsonOutput;
}
Ejemplo n.º 14
0
void TimerData::print(std::ostream& result)
{
    //static ctime_t tmargin = CTime::getTicksPerSec() / 100000;
    std::ostream& out = result;
    out << "Timer: " << id << "\n";
    if (!steps.empty())
    {
        //out << "\nSteps Duration Statistics (in ms) :\n";
        out << " LEVEL      START       NUM         MIN        MAX       MEAN       DEV        TOTAL     PERCENT     ID\n";
        ctime_t ttotal = stepData[AdvancedTimer::IdStep()].ttotal;
        for (unsigned int s=0; s<steps.size(); ++s)
        {
            StepData& data = stepData[steps[s]];
            printVal(out, data.level);
            out << "    ";
            printTime(out, data.tstart, data.numIt);
            out << "    ";
            printVal(out, data.num, (s == 0) ? 1 : nbIter);
            out << "    ";
            printTime(out, data.tmin);
            out << "    ";
            printTime(out, data.tmax);
            out << "    ";
            double mean = (double)data.ttotal / data.num;
            printTime(out, (ctime_t)mean);
            out << "    ";
            printTime(out, (ctime_t)(sqrt((double)data.ttotal2/data.num - mean*mean)));
            out << "    ";
            printTime(out, data.ttotal, (s == 0) ? 1 : nbIter);
            out << "    ";
            printVal(out, 100.0*data.ttotal / (double) ttotal);
            out << "    ";
            if (s == 0)
                out << "TOTAL";
            else
            {
                for(int ii=0; ii<data.level; ii++) out<<".";  // indentation to show the hierarchy level
                out << steps[s];
            }
            out << std::endl;
        }
    }
    if (!vals.empty())
    {
        out << "\nValues Statistics :\n";
        out << " NUM\t  MIN\t  MAX\t MEAN\t  DEV\t TOTAL\tID\n";
        for (unsigned int s=0; s<vals.size(); ++s)
        {
            ValData& data = valData[vals[s]];
            printVal(out, data.num, nbIter);
            out << '\t';
            printVal(out, data.vmin);
            out << '\t';
            printVal(out, data.vmax);
            out << '\t';
            double mean = data.vtotal / data.num;
            printVal(out, mean);
            out << '\t';
            printVal(out, sqrt(data.vtotal2/data.num - mean*mean) );
            out << '\t';
            printVal(out, data.vtotal, nbIter);
            out << '\t';
            out << vals[s];
            out << std::endl;
        }
    }

    //out << "\n==== END ====\n";
    out << std::endl;
}
Ejemplo n.º 15
0
// build a projection basis based on constraint types (bilateral vs others)
// TODO put it in a helper file that can be used by other solvers?
static unsigned projection_bilateral(AssembledSystem::rmat& Q_bilat, AssembledSystem::rmat& Q_unil, const AssembledSystem& sys)
{
    // flag which constraint are bilateral
    static helper::vector<bool> bilateral;
    bilateral.resize( sys.n );
    unsigned nb_bilaterals = 0;

    for(unsigned i=0, off=0, n=sys.compliant.size(); i < n; ++i)
    {
        const AssembledSystem::dofs_type* dofs = sys.compliant[i];
        const AssembledSystem::constraint_type& constraint = sys.constraints[i];

        const unsigned dim = dofs->getDerivDimension();

        for(unsigned k = 0, max = dofs->getSize(); k < max; ++k)
        {
            bool bilat = !constraint.projector.get(); // flag bilateral or not
            if( bilat ) nb_bilaterals += dim;
            const helper::vector<bool>::iterator itoff = bilateral.begin() + off;
            std::fill( itoff, itoff+dim, bilat );
            off += dim;
        }
    }

    if( !nb_bilaterals )  // no bilateral constraints
    {
        return 0;
//        Q_bilat = AssembledSystem::rmat();
//        Q_unil.resize(sys.n, sys.n);
//        Q_unil.setIdentity();
    }
    else if( nb_bilaterals == sys.n ) // every constraints are bilateral,
    {
        Q_bilat.resize(sys.n, sys.n);
        Q_bilat.setIdentity();
        Q_unil = AssembledSystem::rmat();
    }
    else
    {
        Q_bilat.resize( sys.n, nb_bilaterals );
        Q_bilat.reserve(nb_bilaterals);

        unsigned nb_unilaterals = sys.n-nb_bilaterals;
        Q_unil.resize( sys.n, nb_unilaterals );
        Q_unil.reserve(nb_unilaterals);

        unsigned off_bilat = 0;
        unsigned off_unil = 0;
        for( unsigned i = 0 ; i < sys.n ; ++i )
        {
            Q_bilat.startVec(i);
            Q_unil.startVec(i);

            if( bilateral[i] ) Q_bilat.insertBack(i, off_bilat++) = 1;
            else Q_unil.insertBack(i, off_unil++) = 1;
        }

        Q_bilat.finalize();
        Q_unil.finalize();
    }

    return nb_bilaterals;
}