SceneState.cpp
Go to the documentation of this file.
1 #include "SceneState.h"
2 
3 using namespace hrp;
4 using namespace OpenHRP;
5 
6 void SceneState::set(hrp::WorldBase& i_world, OpenHRP::CollisionSequence& i_collisions)
7 {
8  time = i_world.currentTime();
9  bodyStates.resize(i_world.numBodies());
10  for (unsigned int i=0; i<i_world.numBodies(); i++){
11  BodyPtr body = i_world.body(i);
12  bodyStates[i].set(body);
13  }
14  size_t n=0;
15  for (size_t i=0; i<i_collisions.length(); i++){
16  n += i_collisions[i].points.length();
17  }
18  collisions.resize(n);
19  size_t index=0;
20  for (size_t i=0; i<i_collisions.length(); i++){
21  Collision &col = i_collisions[i];
22  for (size_t j=0; j<col.points.length(); j++){
23  CollisionInfo& ci=collisions[index++];
24  CollisionPoint& cp=col.points[j];
25  for (int k=0; k<3; k++){
26  ci.position[k] = cp.position[k];
27  ci.normal[k] = cp.normal[k];
28  }
29  ci.idepth = cp.idepth;
30  }
31  }
32 }
void set(hrp::WorldBase &i_world, OpenHRP::CollisionSequence &i_collisions)
Definition: SceneState.cpp:6
double idepth
Definition: SceneState.h:13
double normal[3]
Definition: SceneState.h:12
png_uint_32 i
double position[3]
Definition: SceneState.h:11
BodyPtr body(int index)
double currentTime(void) const
list index
n
unsigned int numBodies()


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Sat Dec 17 2022 03:52:21