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(nullptr) {}
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 != nullptr;
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 != nullptr;
118  pt_shape = pt_shape->next) {
119  if(cpSpaceShapeQuery(
120  const_cast<CDynamics2DSingleBodyObjectModel*>(this)->
121  GetDynamics2DEngine().GetPhysicsSpace(),
122  pt_shape, nullptr, nullptr) > 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 }
float Real
Collects all ARGoS code.
Definition: datatypes.h:39
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
Basic class for an entity that contains other entities.
This entity is a link to a body in the physics engine.
const SAnchor & GetOriginAnchor() const
Returns a const reference to the origin anchor associated to this entity.
An anchor related to the body of an entity.
Definition: physics_model.h:38
CQuaternion Orientation
The orientation of the anchor wrt the global coordinate system.
Definition: physics_model.h:53
CVector3 Position
The position of the anchor wrt the global coordinate system.
Definition: physics_model.h:51
void RegisterAnchorMethod(const SAnchor &s_anchor, void(MODEL::*pt_method)(SAnchor &))
Registers an anchor method.
CEmbodiedEntity & GetEmbodiedEntity()
Returns the embodied entity associated to this physics model.
const SBoundingBox & GetBoundingBox() const
Returns an axis-aligned box that contains the physics model.
virtual void UpdateEntityStatus()
Updates the status of the associated entity.
It defines the basic type CRadians, used to store an angle value in radians.
Definition: angles.h:42
Real GetValue() const
Returns the value in radians.
Definition: angles.h:111
void ToEulerAngles(CRadians &c_z_angle, CRadians &c_y_angle, CRadians &c_x_angle) const
Definition: quaternion.h:172
CQuaternion & FromAngleAxis(const CRadians &c_angle, const CVector3 &c_vector)
Definition: quaternion.h:126
A 3D vector class.
Definition: vector3.h:31
void SetY(const Real f_y)
Sets the y coordinate of this vector.
Definition: vector3.h:129
Real GetX() const
Returns the x coordinate of this vector.
Definition: vector3.h:105
void SetX(const Real f_x)
Sets the x coordinate of this vector.
Definition: vector3.h:113
void SetZ(const Real f_z)
Sets the z coordinate of this vector.
Definition: vector3.h:145
Real GetY() const
Returns the y coordinate of this vector.
Definition: vector3.h:121
static const CVector3 Z
The z axis.
Definition: vector3.h:42
The base class for models in the dynamics 2D engine.
CDynamics2DEngine & GetDynamics2DEngine()
Returns the dynamics 2D engine state.
Base class for object models with a single body.
CDynamics2DSingleBodyObjectModel(CDynamics2DEngine &c_engine, CComposableEntity &c_entity)
Class constructor.
virtual void MoveTo(const CVector3 &c_position, const CQuaternion &c_orientation)
virtual void UpdateEntityStatus()
Updates the status of the associated entity.
void UpdateOriginAnchor(SAnchor &s_anchor)
Updates the origin anchor associated to the embodied entity.
virtual void SetBody(cpBody *pt_body, Real f_height)
Sets the body and registers the default origin anchor method.
virtual bool IsCollidingWithSomething() const
Returns true if this model is colliding with another model.
virtual void CalculateBoundingBox()
Calculates the axis-aligned box that contains the entire physics model.