2 #include <argos3/core/simulator/entity/composable_entity.h>
12 m_cEntity(c_entity) {}
19 while(!m_vecBodies.empty()) {
21 cpBody* ptBody = m_vecBodies.back().Body;
23 cpShape* ptCurShape = ptBody->shapeList;
26 ptNextShape = ptCurShape->next;
28 cpShapeFree(ptCurShape);
29 ptCurShape = ptNextShape;
35 m_vecBodies.pop_back();
45 cpVect tBodyPos = cpv(c_position.
GetX(), c_position.
GetY());
48 cpFloat tBodyOrient = cZAngle.
GetValue();
50 for(
size_t i = 0; i < m_vecBodies.size(); ++i) {
52 cpBodySetAngle(m_vecBodies[i].Body,
53 tBodyOrient + m_vecBodies[i].OffsetOrient);
55 cpBodySetPos(m_vecBodies[i].Body,
57 cpvrotate(m_vecBodies[i].OffsetPos,
58 m_vecBodies[i].Body->rot)));
76 for(
size_t i = 0; i < m_vecBodies.size(); ++i) {
78 m_vecBodies[i].Body->v = cpvzero;
79 m_vecBodies[i].Body->w = 0.0f;
81 cpBodyResetForces(m_vecBodies[i].Body);
89 if(m_vecBodies.empty())
return;
92 for(
size_t i = 0; i < m_vecBodies.size(); ++i) {
93 tBoundingBox = cpShapeGetBB(m_vecBodies[i].Body->shapeList);
94 for(cpShape* pt_shape = m_vecBodies[i].Body->shapeList->next;
96 pt_shape = pt_shape->next) {
97 cpBB* ptBB = &pt_shape->bb;
98 if(ptBB->l < tBoundingBox.l) tBoundingBox.l = ptBB->l;
99 if(ptBB->b < tBoundingBox.b) tBoundingBox.b = ptBB->b;
100 if(ptBB->r > tBoundingBox.r) tBoundingBox.r = ptBB->r;
101 if(ptBB->t > tBoundingBox.t) tBoundingBox.t = ptBB->t;
103 fMaxHeight =
Max(fMaxHeight, m_vecBodies[i].Height);
117 if(m_vecBodies.empty())
return false;
118 for(
size_t i = 0; i < m_vecBodies.size(); ++i) {
119 for(cpShape* pt_shape = m_vecBodies[i].Body->shapeList;
121 pt_shape = pt_shape->next) {
122 if(cpSpaceShapeQuery(
125 pt_shape, NULL, NULL) > 0) {
137 const cpVect& t_offset_pos,
138 cpFloat t_offset_orient,
141 pt_body->data =
this;
143 m_vecBodies.push_back(
SBody(pt_body,
155 const cpVect& t_offset_pos,
156 cpFloat t_offset_orient,
159 OffsetPos(t_offset_pos),
160 OffsetOrient(t_offset_orient),