GLbody::GLbody(ISceneNode *i_parent, ISceneManager *i_mgr, s32 i_id,
BodyInfo_var i_binfo) :
ISceneNode(i_parent, i_mgr, i_id) {
setAutomaticCulling(scene::EAC_OFF);
LinkInfoSequence_var lis = i_binfo->links();
//std::cout << "creating links" << std::endl;
for (unsigned int i=0; i<lis->length(); i++) {
m_links.push_back(
new GLlink(i_mgr->getRootSceneNode(), i_mgr,
-1, lis[i], i_binfo));
}
//std::cout << "creating a tree" << std::endl;
// setup tree
for (unsigned int i=0; i<m_links.size(); i++) {
const LinkInfo &li = lis[i];
if (li.parentIndex < 0) {
m_root = m_links[i];
addChild(m_links[i]);
}
for (unsigned int j=0; j<li.childIndices.length(); j++) {
m_links[i]->addChild(m_links[li.childIndices[j]]);
}
}
//std::cout << "done" << std::endl;
}
void SimScheduler::loadModels(int argc, char *argv[])
{
floor_ = hrp::loadBodyInfo(floorfilename_.c_str(), argc, argv);
if(!floor_)
{
cerr << "ModelLoader: " << floorfilename_ << " cannot be loaded" << endl;
return;
}
body_ = hrp::loadBodyInfo(robotfilename_.c_str(), argc, argv);
if(!body_)
{
cerr << "ModelLoader: " << robotfilename_ << " cannot be loaded" << endl;
return;
}
double lmass=0.0;
LinkInfoSequence_var links = body_->links();
for(unsigned int i=0;i<links->length();i++)
{
lmass+=links[i].mass;
};
std::cout << "Mass of " <<body_->name() << " : " << lmass << std::endl;
}
ColdetBody::ColdetBody(BodyInfo_ptr bodyInfo)
{
LinkInfoSequence_var links = bodyInfo->links();
ShapeInfoSequence_var shapes = bodyInfo->shapes();
int numLinks = links->length();
linkColdetModels.resize(numLinks);
for(int linkIndex = 0; linkIndex < numLinks ; ++linkIndex){
LinkInfo& linkInfo = links[linkIndex];
int totalNumTriangles = 0;
int totalNumVertices = 0;
const TransformedShapeIndexSequence& shapeIndices = linkInfo.shapeIndices;
short shapeIndex;
double R[9], p[3];
unsigned int nshape = shapeIndices.length();
for(int i=0; i < shapeIndices.length(); i++){
shapeIndex = shapeIndices[i].shapeIndex;
const DblArray12 &tform = shapeIndices[i].transformMatrix;
R[0] = tform[0]; R[1] = tform[1]; R[2] = tform[2]; p[0] = tform[3];
R[3] = tform[4]; R[4] = tform[5]; R[5] = tform[6]; p[1] = tform[7];
R[6] = tform[8]; R[7] = tform[9]; R[8] = tform[10]; p[2] = tform[11];
const ShapeInfo& shapeInfo = shapes[shapeIndex];
totalNumTriangles += shapeInfo.triangles.length() / 3;
totalNumVertices += shapeInfo.vertices.length() / 3;
}
const SensorInfoSequence& sensors = linkInfo.sensors;
for (unsigned int i=0; i<sensors.length(); i++){
const SensorInfo &sinfo = sensors[i];
const TransformedShapeIndexSequence tsis = sinfo.shapeIndices;
nshape += tsis.length();
for (unsigned int j=0; j<tsis.length(); j++){
shapeIndex = tsis[j].shapeIndex;
const DblArray12 &tform = tsis[j].transformMatrix;
R[0] = tform[0]; R[1] = tform[1]; R[2] = tform[2]; p[0] = tform[3];
R[3] = tform[4]; R[4] = tform[5]; R[5] = tform[6]; p[1] = tform[7];
R[6] = tform[8]; R[7] = tform[9]; R[8] = tform[10]; p[2] = tform[11];
const ShapeInfo& shapeInfo = shapes[shapeIndex];
totalNumTriangles += shapeInfo.triangles.length() / 3;
totalNumVertices += shapeInfo.vertices.length() / 3 ;
}
}
//int totalNumVertices = totalNumTriangles * 3;
ColdetModelPtr coldetModel(new ColdetModel());
coldetModel->setName(std::string(linkInfo.name));
if(totalNumTriangles > 0){
coldetModel->setNumVertices(totalNumVertices);
coldetModel->setNumTriangles(totalNumTriangles);
if (nshape == 1){
addLinkPrimitiveInfo(coldetModel, R, p, shapes[shapeIndex]);
}
addLinkVerticesAndTriangles(coldetModel, linkInfo, shapes);
coldetModel->build();
}
linkColdetModels[linkIndex] = coldetModel;
linkNameToColdetModelMap.insert(make_pair(linkInfo.name, coldetModel));
cout << linkInfo.name << " has "<< totalNumTriangles << " triangles." << endl;
}
}
