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;
91 Real fMaxHeight = 0.0f;
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(
123 const_cast<CDynamics2DMultiBodyObjectModel*>(
this)->
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),
T Max(const T &t_v1, const T &t_v2)
Returns the bigger of the two passed arguments.
float Real
Collects all ARGoS code.
virtual bool IsCollidingWithSomething() const
Returns true if this model is colliding with another model.
Real GetX() const
Returns the x coordinate of this vector.
Real GetY() const
Returns the y coordinate of this vector.
virtual void MoveTo(const CVector3 &c_position, const CQuaternion &c_orientation)
SBody(cpBody *pt_body, const cpVect &t_offset_pos, cpFloat t_offset_orient, Real f_height)
This entity is a link to a body in the physics engine.
It defines the basic type CRadians, used to store an angle value in radians.
virtual void AddBody(cpBody *pt_body, const cpVect &t_offset_pos, cpFloat t_offset_orient, Real f_height)
Adds a body.
virtual void UpdateEntityStatus()
Updates the status of the associated entity.
Basic class for an entity that contains other entities.
void SetX(const Real f_x)
Sets the x coordinate of this vector.
CDynamics2DEngine & GetDynamics2DEngine()
Returns the dynamics 2D engine state.
void ToEulerAngles(CRadians &c_z_angle, CRadians &c_y_angle, CRadians &c_x_angle) const
CEmbodiedEntity & GetEmbodiedEntity()
Returns the embodied entity associated to this physics model.
void SetY(const Real f_y)
Sets the y coordinate of this vector.
The base class for models in the dynamics 2D engine.
const SBoundingBox & GetBoundingBox() const
Returns an axis-aligned box that contains the physics model.
CDynamics2DMultiBodyObjectModel(CDynamics2DEngine &c_engine, CComposableEntity &c_entity)
Class constructor.
virtual ~CDynamics2DMultiBodyObjectModel()
Class destructor.
The namespace containing all the ARGoS related code.
void SetZ(const Real f_z)
Sets the z coordinate of this vector.
virtual void CalculateBoundingBox()
Calculates the axis-aligned box that contains the entire physics model.