ab8f16961e
Apply clang-format-all.sh using the _clang-format file through all the cpp/.h files. make sure not to apply it to certain serialization structures, since some parser expects the * as part of the name, instead of type. This commit contains no other changes aside from adding and applying clang-format-all.sh
78 lines
1.7 KiB
C++
78 lines
1.7 KiB
C++
#include "MultiBodyTreeDebugGraph.hpp"
|
|
|
|
#include <cstdio>
|
|
|
|
namespace btInverseDynamics
|
|
{
|
|
int writeGraphvizDotFile(const MultiBodyTree* tree, const MultiBodyNameMap* map,
|
|
const char* filename)
|
|
{
|
|
if (0x0 == tree)
|
|
{
|
|
bt_id_error_message("tree pointer is null\n");
|
|
return -1;
|
|
}
|
|
if (0x0 == filename)
|
|
{
|
|
bt_id_error_message("filename is null\n");
|
|
return -1;
|
|
}
|
|
|
|
FILE* fp = fopen(filename, "w");
|
|
if (NULL == fp)
|
|
{
|
|
bt_id_error_message("cannot open file %s for writing\n", filename);
|
|
return -1;
|
|
}
|
|
fprintf(fp,
|
|
"// to generate postscript file, run dot -Tps %s -o %s.ps\n"
|
|
"// details see graphviz documentation at http://graphviz.org\n"
|
|
"digraph tree {\n",
|
|
filename, filename);
|
|
|
|
for (int body = 0; body < tree->numBodies(); body++)
|
|
{
|
|
std::string name;
|
|
if (0x0 != map)
|
|
{
|
|
if (-1 == map->getBodyName(body, &name))
|
|
{
|
|
bt_id_error_message("can't get name of body %d\n", body);
|
|
return -1;
|
|
}
|
|
fprintf(fp, " %d [label=\"%d/%s\"];\n", body, body, name.c_str());
|
|
}
|
|
}
|
|
for (int body = 0; body < tree->numBodies(); body++)
|
|
{
|
|
int parent;
|
|
const char* joint_type;
|
|
int qi;
|
|
if (-1 == tree->getParentIndex(body, &parent))
|
|
{
|
|
bt_id_error_message("indexing error\n");
|
|
return -1;
|
|
}
|
|
if (-1 == tree->getJointTypeStr(body, &joint_type))
|
|
{
|
|
bt_id_error_message("indexing error\n");
|
|
return -1;
|
|
}
|
|
if (-1 == tree->getDoFOffset(body, &qi))
|
|
{
|
|
bt_id_error_message("indexing error\n");
|
|
return -1;
|
|
}
|
|
if (-1 != parent)
|
|
{
|
|
fprintf(fp, " %d -> %d [label= \"type:%s, q=%d\"];\n", parent, body,
|
|
joint_type, qi);
|
|
}
|
|
}
|
|
|
|
fprintf(fp, "}\n");
|
|
fclose(fp);
|
|
return 0;
|
|
}
|
|
} // namespace btInverseDynamics
|