ARGoS  3
A parallel, multi-engine simulator for swarm robotics
dynamics2d_single_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  m_ptBody(NULL) {}
14 
15  /****************************************/
16  /****************************************/
17 
19  bool bIsStatic = cpBodyIsStatic(m_ptBody);
20  /* Dispose of shapes */
21  cpShape* ptCurShape = m_ptBody->shapeList;
22  cpShape* ptNextShape;
23  while(ptCurShape) {
24  ptNextShape = ptCurShape->next;
25  cpSpaceRemoveShape(GetDynamics2DEngine().GetPhysicsSpace(), ptCurShape);
26  cpShapeFree(ptCurShape);
27  ptCurShape = ptNextShape;
28  }
29  /* Dispose of body */
30  if(! bIsStatic)
31  cpSpaceRemoveBody(GetDynamics2DEngine().GetPhysicsSpace(), m_ptBody);
32  cpBodyFree(m_ptBody);
33  /* Reindex space */
34  if(bIsStatic) cpSpaceReindexStatic(GetDynamics2DEngine().GetPhysicsSpace());
35  }
36 
37  /****************************************/
38  /****************************************/
39 
41  const CQuaternion& c_orientation) {
42  /* Move the body to the desired position */
43  m_ptBody->p = cpv(c_position.GetX(), c_position.GetY());
44  CRadians cXAngle, cYAngle, cZAngle;
45  c_orientation.ToEulerAngles(cZAngle, cYAngle, cXAngle);
46  cpBodySetAngle(m_ptBody, cZAngle.GetValue());
47  /* Update shape index */
48  if(cpBodyIsStatic(m_ptBody)) {
49  cpBB tBoundingBox = cpShapeGetBB(m_ptBody->shapeList);
50  cpSpaceReindexStatic(GetDynamics2DEngine().GetPhysicsSpace());
51  tBoundingBox = cpShapeGetBB(m_ptBody->shapeList);
52  }
53  else {
54  cpSpaceReindexShapesForBody(GetDynamics2DEngine().GetPhysicsSpace(), m_ptBody);
55  }
56  /* Update ARGoS entity state */
58  }
59 
60  /****************************************/
61  /****************************************/
62 
64  /* Nothing to do for a static body */
65  if(cpBodyIsStatic(m_ptBody)) return;
66  /* Reset body position */
67  const CVector3& cPosition = GetEmbodiedEntity().GetOriginAnchor().Position;
68  m_ptBody->p = cpv(cPosition.GetX(), cPosition.GetY());
69  /* Reset body orientation */
70  CRadians cXAngle, cYAngle, cZAngle;
71  GetEmbodiedEntity().GetOriginAnchor().Orientation.ToEulerAngles(cZAngle, cYAngle, cXAngle);
72  cpBodySetAngle(m_ptBody, cZAngle.GetValue());
73  /* Zero speed and applied forces */
74  m_ptBody->v = cpvzero;
75  m_ptBody->w = 0.0f;
76  cpBodyResetForces(m_ptBody);
77  /* Update bounding box */
78  cpSpaceReindexShapesForBody(GetDynamics2DEngine().GetPhysicsSpace(), m_ptBody);
80  }
81 
82  /****************************************/
83  /****************************************/
84 
86  cpBB tBoundingBox = cpShapeGetBB(m_ptBody->shapeList);
87  for(cpShape* pt_shape = m_ptBody->shapeList->next;
88  pt_shape != NULL;
89  pt_shape = pt_shape->next) {
90  cpBB* ptBB = &pt_shape->bb;
91  if(ptBB->l < tBoundingBox.l) tBoundingBox.l = ptBB->l;
92  if(ptBB->b < tBoundingBox.b) tBoundingBox.b = ptBB->b;
93  if(ptBB->r > tBoundingBox.r) tBoundingBox.r = ptBB->r;
94  if(ptBB->t > tBoundingBox.t) tBoundingBox.t = ptBB->t;
95  }
96  GetBoundingBox().MinCorner.SetX(tBoundingBox.l);
97  GetBoundingBox().MinCorner.SetY(tBoundingBox.b);
98  GetBoundingBox().MaxCorner.SetX(tBoundingBox.r);
99  GetBoundingBox().MaxCorner.SetY(tBoundingBox.t);
100  }
101 
102  /****************************************/
103  /****************************************/
104 
106  /* Nothing to do for a static body */
107  if(!cpBodyIsStatic(m_ptBody)) {
109  }
110  }
111 
112  /****************************************/
113  /****************************************/
114 
116  for(cpShape* pt_shape = m_ptBody->shapeList;
117  pt_shape != NULL;
118  pt_shape = pt_shape->next) {
119  if(cpSpaceShapeQuery(
120  const_cast<CDynamics2DSingleBodyObjectModel*>(this)->
121  GetDynamics2DEngine().GetPhysicsSpace(),
122  pt_shape, NULL, NULL) > 0) {
123  return true;
124  }
125  }
126  return false;
127  }
128 
129  /****************************************/
130  /****************************************/
131 
133  Real f_height) {
134  /* Set the body and its data field for ray queries */
135  m_ptBody = pt_body;
136  m_ptBody->data = this;
137  /* Register the origin anchor update method */
138  RegisterAnchorMethod(GetEmbodiedEntity().GetOriginAnchor(),
140  /* Calculate the bounding box */
141  GetBoundingBox().MinCorner.SetZ(GetEmbodiedEntity().GetOriginAnchor().Position.GetZ());
142  GetBoundingBox().MaxCorner.SetZ(GetEmbodiedEntity().GetOriginAnchor().Position.GetZ() + f_height);
144  }
145 
146  /****************************************/
147  /****************************************/
148 
150  s_anchor.Position.SetX(m_ptBody->p.x);
151  s_anchor.Position.SetY(m_ptBody->p.y);
152  s_anchor.Orientation.FromAngleAxis(CRadians(m_ptBody->a), CVector3::Z);
153  }
154 
155  /****************************************/
156  /****************************************/
157 
158 }
argos::CDynamics2DSingleBodyObjectModel::~CDynamics2DSingleBodyObjectModel
virtual ~CDynamics2DSingleBodyObjectModel()
Class destructor.
Definition: dynamics2d_single_body_object_model.cpp:18
argos::CEmbodiedEntity::GetOriginAnchor
const SAnchor & GetOriginAnchor() const
Returns a const reference to the origin anchor associated to this entity.
Definition: embodied_entity.h:119
argos::SAnchor::Orientation
CQuaternion Orientation
The orientation of the anchor wrt the global coordinate system.
Definition: physics_model.h:53
argos::CDynamics2DSingleBodyObjectModel::UpdateOriginAnchor
void UpdateOriginAnchor(SAnchor &s_anchor)
Updates the origin anchor associated to the embodied entity.
Definition: dynamics2d_single_body_object_model.cpp:149
argos::CDynamics2DSingleBodyObjectModel::CalculateBoundingBox
virtual void CalculateBoundingBox()
Calculates the axis-aligned box that contains the entire physics model.
Definition: dynamics2d_single_body_object_model.cpp:85
argos::CVector3::SetX
void SetX(const Real f_x)
Sets the x coordinate of this vector.
Definition: vector3.h:101
argos
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
argos::CVector3
A 3D vector class.
Definition: vector3.h:29
argos::CDynamics2DModel::GetDynamics2DEngine
CDynamics2DEngine & GetDynamics2DEngine()
Returns the dynamics 2D engine state.
Definition: dynamics2d_model.h:48
argos::CRadians
It defines the basic type CRadians, used to store an angle value in radians.
Definition: angles.h:42
argos::CComposableEntity
Basic class for an entity that contains other entities.
Definition: composable_entity.h:32
argos::CPhysicsModel::UpdateEntityStatus
virtual void UpdateEntityStatus()
Updates the status of the associated entity.
Definition: physics_model.cpp:55
argos::CDynamics2DSingleBodyObjectModel::CDynamics2DSingleBodyObjectModel
CDynamics2DSingleBodyObjectModel(CDynamics2DEngine &c_engine, CComposableEntity &c_entity)
Class constructor.
Definition: dynamics2d_single_body_object_model.cpp:9
argos::SBoundingBox::MinCorner
CVector3 MinCorner
Definition: physics_model.h:88
argos::SBoundingBox::MaxCorner
CVector3 MaxCorner
Definition: physics_model.h:89
argos::CDynamics2DSingleBodyObjectModel
Base class for object models with a single body.
Definition: dynamics2d_single_body_object_model.h:47
argos::CVector3::Z
static const CVector3 Z
The z axis.
Definition: vector3.h:40
argos::CDynamics2DEngine
Definition: dynamics2d_engine.h:42
argos::CEmbodiedEntity
This entity is a link to a body in the physics engine.
Definition: embodied_entity.h:48
argos::CQuaternion::FromAngleAxis
CQuaternion & FromAngleAxis(const CRadians &c_angle, const CVector3 &c_vector)
Definition: quaternion.h:125
argos::CVector3::GetX
Real GetX() const
Returns the x coordinate of this vector.
Definition: vector3.h:93
argos::CRadians::GetValue
Real GetValue() const
Returns the value in radians.
Definition: angles.h:111
argos::CVector3::SetZ
void SetZ(const Real f_z)
Sets the z coordinate of this vector.
Definition: vector3.h:133
argos::CQuaternion::ToEulerAngles
void ToEulerAngles(CRadians &c_z_angle, CRadians &c_y_angle, CRadians &c_x_angle) const
Definition: quaternion.h:171
argos::CQuaternion
Definition: quaternion.h:14
argos::CDynamics2DSingleBodyObjectModel::IsCollidingWithSomething
virtual bool IsCollidingWithSomething() const
Returns true if this model is colliding with another model.
Definition: dynamics2d_single_body_object_model.cpp:115
argos::SAnchor
An anchor related to the body of an entity.
Definition: physics_model.h:38
argos::CDynamics2DSingleBodyObjectModel::UpdateEntityStatus
virtual void UpdateEntityStatus()
Updates the status of the associated entity.
Definition: dynamics2d_single_body_object_model.cpp:105
argos::CPhysicsModel::RegisterAnchorMethod
void RegisterAnchorMethod(const SAnchor &s_anchor, void(MODEL::*pt_method)(SAnchor &))
Registers an anchor method.
Definition: physics_model.h:325
argos::CDynamics2DSingleBodyObjectModel::SetBody
virtual void SetBody(cpBody *pt_body, Real f_height)
Sets the body and registers the default origin anchor method.
Definition: dynamics2d_single_body_object_model.cpp:132
argos::CDynamics2DModel
The base class for models in the dynamics 2D engine.
Definition: dynamics2d_model.h:27
argos::CPhysicsModel::GetBoundingBox
const SBoundingBox & GetBoundingBox() const
Returns an axis-aligned box that contains the physics model.
Definition: physics_model.h:198
dynamics2d_single_body_object_model.h
argos::CDynamics2DSingleBodyObjectModel::Reset
virtual void Reset()
Definition: dynamics2d_single_body_object_model.cpp:63
argos::CVector3::GetY
Real GetY() const
Returns the y coordinate of this vector.
Definition: vector3.h:109
argos::CDynamics2DSingleBodyObjectModel::MoveTo
virtual void MoveTo(const CVector3 &c_position, const CQuaternion &c_orientation)
Definition: dynamics2d_single_body_object_model.cpp:40
argos::SAnchor::Position
CVector3 Position
The position of the anchor wrt the global coordinate system.
Definition: physics_model.h:51
Real
float Real
Collects all ARGoS code.
Definition: datatypes.h:39
argos::CVector3::SetY
void SetY(const Real f_y)
Sets the y coordinate of this vector.
Definition: vector3.h:117
argos::CPhysicsModel::GetEmbodiedEntity
CEmbodiedEntity & GetEmbodiedEntity()
Returns the embodied entity associated to this physics model.
Definition: physics_model.h:133