dynamics2d_multi_body_object_model.cpp
Go to the documentation of this file.
2 #include <argos3/core/simulator/entity/composable_entity.h>
3 
4 namespace argos {
5 
6  /****************************************/
7  /****************************************/
8 
10  CComposableEntity& c_entity) :
11  CDynamics2DModel(c_engine, c_entity.GetComponent<CEmbodiedEntity>("body")),
12  m_cEntity(c_entity) {}
13 
14  /****************************************/
15  /****************************************/
16 
18  /* Dispose of shapes and bodies */
19  while(!m_vecBodies.empty()) {
20  /* Get pointer to body */
21  cpBody* ptBody = m_vecBodies.back().Body;
22  /* Remove all of its shapes */
23  cpShape* ptCurShape = ptBody->shapeList;
24  cpShape* ptNextShape;
25  while(ptCurShape) {
26  ptNextShape = ptCurShape->next;
27  cpSpaceRemoveShape(GetDynamics2DEngine().GetPhysicsSpace(), ptCurShape);
28  cpShapeFree(ptCurShape);
29  ptCurShape = ptNextShape;
30  }
31  /* Dispose of body */
32  cpSpaceRemoveBody(GetDynamics2DEngine().GetPhysicsSpace(), ptBody);
33  cpBodyFree(ptBody);
34  /* Dispose of struct */
35  m_vecBodies.pop_back();
36  }
37  }
38 
39  /****************************************/
40  /****************************************/
41 
43  const CQuaternion& c_orientation) {
44  /* Set target position and orientation */
45  cpVect tBodyPos = cpv(c_position.GetX(), c_position.GetY());
46  CRadians cXAngle, cYAngle, cZAngle;
47  c_orientation.ToEulerAngles(cZAngle, cYAngle, cXAngle);
48  cpFloat tBodyOrient = cZAngle.GetValue();
49  /* For each body: */
50  for(size_t i = 0; i < m_vecBodies.size(); ++i) {
51  /* Set body orientation at anchor */
52  cpBodySetAngle(m_vecBodies[i].Body,
53  tBodyOrient + m_vecBodies[i].OffsetOrient);
54  /* Set body position at anchor */
55  cpBodySetPos(m_vecBodies[i].Body,
56  cpvadd(tBodyPos,
57  cpvrotate(m_vecBodies[i].OffsetPos,
58  m_vecBodies[i].Body->rot)));
59 
60  /* Update shape index */
61  cpSpaceReindexShapesForBody(GetDynamics2DEngine().GetPhysicsSpace(),
62  m_vecBodies[i].Body);
63  }
64  /* Update ARGoS entity state */
66  }
67 
68  /****************************************/
69  /****************************************/
70 
72  /* Reset body position */
73  MoveTo(GetEmbodiedEntity().GetOriginAnchor().Position,
74  GetEmbodiedEntity().GetOriginAnchor().Orientation);
75  /* For each body: */
76  for(size_t i = 0; i < m_vecBodies.size(); ++i) {
77  /* Zero the speeds */
78  m_vecBodies[i].Body->v = cpvzero;
79  m_vecBodies[i].Body->w = 0.0f;
80  /* Zero forces and torques */
81  cpBodyResetForces(m_vecBodies[i].Body);
82  }
83  }
84 
85  /****************************************/
86  /****************************************/
87 
89  if(m_vecBodies.empty()) return;
90  cpBB tBoundingBox;
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;
95  pt_shape != NULL;
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;
102  }
103  fMaxHeight = Max(fMaxHeight, m_vecBodies[i].Height);
104  }
105  GetBoundingBox().MinCorner.SetX(tBoundingBox.l);
106  GetBoundingBox().MinCorner.SetY(tBoundingBox.b);
107  GetBoundingBox().MinCorner.SetZ(GetDynamics2DEngine().GetElevation());
108  GetBoundingBox().MaxCorner.SetX(tBoundingBox.r);
109  GetBoundingBox().MaxCorner.SetY(tBoundingBox.t);
110  GetBoundingBox().MaxCorner.SetZ(GetDynamics2DEngine().GetElevation() + fMaxHeight);
111  }
112 
113  /****************************************/
114  /****************************************/
115 
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;
120  pt_shape != NULL;
121  pt_shape = pt_shape->next) {
122  if(cpSpaceShapeQuery(
123  const_cast<CDynamics2DMultiBodyObjectModel*>(this)->
124  GetDynamics2DEngine().GetPhysicsSpace(),
125  pt_shape, NULL, NULL) > 0) {
126  return true;
127  }
128  }
129  }
130  return false;
131  }
132 
133  /****************************************/
134  /****************************************/
135 
137  const cpVect& t_offset_pos,
138  cpFloat t_offset_orient,
139  Real f_height) {
140  /* Set the body and its data field for ray queries */
141  pt_body->data = this;
142  /* Add body to list */
143  m_vecBodies.push_back(SBody(pt_body,
144  t_offset_pos,
145  t_offset_orient,
146  f_height));
147  /* Calculate the bounding box */
149  }
150 
151  /****************************************/
152  /****************************************/
153 
155  const cpVect& t_offset_pos,
156  cpFloat t_offset_orient,
157  Real f_height) :
158  Body(pt_body),
159  OffsetPos(t_offset_pos),
160  OffsetOrient(t_offset_orient),
161  Height(f_height) {}
162 
163  /****************************************/
164  /****************************************/
165 
166 }
T Max(const T &t_v1, const T &t_v2)
Returns the bigger of the two passed arguments.
Definition: general.h:95
A 3D vector class.
Definition: vector3.h:29
float Real
Collects all ARGoS code.
Definition: datatypes.h:39
virtual bool IsCollidingWithSomething() const
Returns true if this model is colliding with another model.
Real GetX() const
Returns the x coordinate of this vector.
Definition: vector3.h:93
Real GetY() const
Returns the y coordinate of this vector.
Definition: vector3.h:109
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.
Definition: angles.h:42
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.
Definition: vector3.h:101
CDynamics2DEngine & GetDynamics2DEngine()
Returns the dynamics 2D engine state.
void ToEulerAngles(CRadians &c_z_angle, CRadians &c_y_angle, CRadians &c_x_angle) const
Definition: quaternion.h:172
CEmbodiedEntity & GetEmbodiedEntity()
Returns the embodied entity associated to this physics model.
void SetY(const Real f_y)
Sets the y coordinate of this vector.
Definition: vector3.h:117
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.
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
void SetZ(const Real f_z)
Sets the z coordinate of this vector.
Definition: vector3.h:133
virtual void CalculateBoundingBox()
Calculates the axis-aligned box that contains the entire physics model.