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();
}
void TimerData::clear()
{
nbIter = 0;
steps.clear();
stepData.clear();
vals.clear();
valData.clear();
}
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;
}
}
}
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;
}
}
/**
* 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();
}
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();
}
}
}
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]);
}
}
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);
}
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;
}
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;
}
}
}
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;
}
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;
}
// 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;
}