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 }