void dumpInterp(Interp_t* i)
{
Element_t* e;
fprintf(WmlsStdout, "-------------------------------\n");
#ifdef DUMP_OPCODE
dumpOpcode(i);
#endif
for (e = i->base; e <= i->OperandPtr; e++)
{
dumpElement(e);
}
}
TreeStream& TreeStream::operator<<(const RigidBodyDynamics::Model& model)
{
(*m_stream) << color::cyan();
(*m_stream) << "============================= TREE DUMP =============================\n";
(*m_stream) << color::reset();
dumpElement(model, 0, indent());
(*m_stream) << color::cyan();
(*m_stream) << "=====================================================================\n";
(*m_stream) << color::reset() << std::flush;
return *this;
}
void TreeStream::dumpElement(const RigidBodyDynamics::Model& model, int index, indent ind)
{
const RigidBodyDynamics::Body& body = model.mBodies[index];
(*m_stream) << ind << "Joint at ";
dumpTf(*m_stream, model.X_T[index]);
(*m_stream) << ": " << model.mJoints[index] << "\n";
(*m_stream) << ind << "connected to Body '" << color::brown() << model.GetBodyName(index) << color::reset() << "'\n";
(*m_stream) << ind << "Inertial: " << std::fixed << body.mMass << "kg at " << body.mCenterOfMass << "\n";
for(size_t i = 0; i < model.mu[index].size(); ++i)
{
dumpElement(model, model.mu[index][i], ind.branch());
}
}
static void
iterateThroughElements(DCM_OBJECT** obj, size_t NL)
{
CONDITION cond;
DCM_ELEMENT *e;
DCM_SEQUENCE_ITEM *sq;
cond = DCM_GetFirstElement(obj, &e);
if (cond != DCM_NORMAL) {
COND_DumpConditions();
return;
}
if(e->representation == DCM_SQ){
indentXML(NL);
startXMLTag(e, NL);
printf("\n");
sq = (DCM_SEQUENCE_ITEM*)LST_Head(&e->d.sq);
if (sq != NULL)
(void)LST_Position(&e->d.sq, sq);
while (sq != NULL) {
iterateThroughElements(&sq->object, NL+1);
sq = (DCM_SEQUENCE_ITEM*)LST_Next(&e->d.sq);
}
indentXML(NL);
endXMLTag(e);
printf("\n");
}
else{
startXMLTag(e, NL);
dumpElement(e);
endXMLTag(e);
printf("\n");
}
while (cond == DCM_NORMAL) {
cond = DCM_GetNextElement(obj, &e);
if (cond == DCM_NORMAL) {
if(e->representation == DCM_SQ){
indentXML(NL);
startXMLTag(e, NL);
printf("\n");
sq = (DCM_SEQUENCE_ITEM*)LST_Head(&e->d.sq);
if (sq != NULL)
(void)LST_Position(&e->d.sq, sq);
while (sq != NULL) {
iterateThroughElements(&sq->object, NL+1);
sq = (DCM_SEQUENCE_ITEM*)LST_Next(&e->d.sq);
}
indentXML(NL);
endXMLTag(e);
printf("\n");
}
else{
startXMLTag(e, NL);
dumpElement(e);
endXMLTag(e);
printf("\n");
}
}
}
if (cond != DCM_GETNEXTELEMENTCOMPLETE) {
fprintf(stderr, "Unable to get all elements\n");
}
}
