OnlineViewerUtil.cpp
Go to the documentation of this file.
00001 #include <hrpModel/Link.h>
00002 #include "OnlineViewerUtil.h"
00003 
00004 using namespace hrp;
00005 using namespace OpenHRP;
00006 
00007 void  setupCharacterPosition(CharacterPosition &characterPosition, BodyPtr body) 
00008 {
00009     characterPosition.characterName = CORBA::string_dup(body->name().c_str());
00010     int numLinks = body->numLinks();
00011     characterPosition.linkPositions.length(numLinks);
00012 }
00013 
00014 void  updateCharacterPosition(CharacterPosition &characterPosition, BodyPtr body) 
00015 {
00016     for(unsigned int j=0; j < body->numLinks(); ++j) {
00017         LinkPosition &linkPosition = characterPosition.linkPositions[j];
00018         Link* link = body->link(j);
00019 
00020         setVector3(link->p, linkPosition.p);
00021         setMatrix33ToRowMajorArray(link->attitude(), linkPosition.R);
00022     }
00023 }
00024 
00025 void getWorldState(WorldState& state, WorldBase& world)
00026 {
00027     state.time = world.currentTime();
00028     for (unsigned int i=0; i<world.numBodies(); i++){
00029         updateCharacterPosition(state.characterPositions[i], world.body(i));
00030     }
00031 }
00032 
00033 void initWorldState(WorldState& state,  WorldBase& world)
00034 {
00035     state.characterPositions.length(world.numBodies());
00036     for (unsigned int i=0; i<world.numBodies(); i++){
00037         setupCharacterPosition(state.characterPositions[i], world.body(i));
00038     }
00039 }
00040 
00041 void initWorldState(WorldState& state,  WorldBase& world,
00042                     std::vector<ColdetLinkPairPtr>& pairs)
00043 {
00044     initWorldState(state, world);
00045 
00046     OpenHRP::CollisionSequence& collisions = state.collisions;
00047 
00048     collisions.length(pairs.size());
00049     for(size_t colIndex=0; colIndex < pairs.size(); ++colIndex){
00050         hrp::ColdetLinkPairPtr linkPair = pairs[colIndex];
00051         hrp::Link *link0 = linkPair->link(0);
00052         hrp::Link *link1 = linkPair->link(1);
00053         OpenHRP::LinkPair& pair = collisions[colIndex].pair;
00054         pair.charName1 = CORBA::string_dup(link0->body->name().c_str());
00055         pair.charName2 = CORBA::string_dup(link1->body->name().c_str());
00056         pair.linkName1 = CORBA::string_dup(link0->name.c_str());
00057         pair.linkName2 = CORBA::string_dup(link1->name.c_str());
00058     }
00059 }


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Apr 11 2019 03:30:18