physics_engine.cpp
Go to the documentation of this file.
1 
7 #include <cstdlib>
8 #include "physics_engine.h"
9 #include <argos3/core/utility/logging/argos_log.h>
10 #include <argos3/core/utility/math/vector3.h>
11 #include <argos3/core/utility/string_utilities.h>
12 #include <argos3/core/simulator/simulator.h>
13 #include <argos3/core/simulator/space/space.h>
14 #include <argos3/core/simulator/entity/entity.h>
15 
16 namespace argos {
17 
18  /****************************************/
19  /****************************************/
20 
22  const CRay3& c_ray) {
23  /* This variable is instantiated at the first call of this function, once and forever */
24  static CSimulator& cSimulator = CSimulator::GetInstance();
25  /* Clear data */
26  t_data.clear();
27  /* Create a reference to the vector of physics engines */
28  CPhysicsEngine::TVector& vecEngines = cSimulator.GetPhysicsEngines();
29  /* Ask each engine to perform the ray query */
30  for(size_t i = 0; i < vecEngines.size(); ++i)
31  vecEngines[i]->CheckIntersectionWithRay(t_data, c_ray);
32  /* Remove duplicates */
33  // TODO
34  /* Return true if an intersection was found */
35  return !t_data.empty();
36  }
37 
38  /****************************************/
39  /****************************************/
40 
42  const CRay3& c_ray) {
43  /* Initialize s_item */
44  s_item.IntersectedEntity = NULL;
45  s_item.TOnRay = 1.0f;
46  /* Perform full ray query */
49  /* Go through intersections and find the closest */
50  for(size_t i = 0; i < tData.size(); ++i) {
51  if(s_item.TOnRay > tData[i].TOnRay)
52  s_item = tData[i];
53  }
54  /* Return true if an intersection was found */
55  return (s_item.IntersectedEntity != NULL);
56  }
57 
58  /****************************************/
59  /****************************************/
60 
62  const CRay3& c_ray,
63  CEmbodiedEntity& c_entity) {
64  /* Initialize s_item */
65  s_item.IntersectedEntity = NULL;
66  s_item.TOnRay = 1.0f;
67  /* Perform full ray query */
70  /* Go through intersections and find the closest */
71  for(size_t i = 0; i < tData.size(); ++i) {
72  if(s_item.TOnRay > tData[i].TOnRay &&
73  &c_entity != tData[i].IntersectedEntity) {
74  s_item = tData[i];
75  }
76  }
77  /* Return true if an intersection was found */
78  return (s_item.IntersectedEntity != NULL);
79  }
80 
81  /****************************************/
82  /****************************************/
83 
84  /* The default value of the simulation clock tick */
85  Real CPhysicsEngine::m_fSimulationClockTick = 0.1f;
86  Real CPhysicsEngine::m_fInverseSimulationClockTick = 1.0f / CPhysicsEngine::m_fSimulationClockTick;
87 
88  /****************************************/
89  /****************************************/
90 
92  TopFace(NULL),
93  BottomFace(NULL) {
94  }
95 
96  /****************************************/
97  /****************************************/
98 
100  try {
101  /* Parse top face, if specified */
102  if(NodeExists(t_node, "top")) {
103  TConfigurationNode& tNode = GetNode(t_node, "top");
104  TopFace = new SHorizontalFace;
105  GetNodeAttribute(tNode, "height", TopFace->Height);
106  }
107  /* Parse bottom face, if specified */
108  if(NodeExists(t_node, "bottom")) {
109  TConfigurationNode& tNode = GetNode(t_node, "bottom");
110  BottomFace = new SHorizontalFace;
111  GetNodeAttribute(tNode, "height", BottomFace->Height);
112  }
113  /* Parse side faces, if specified */
114  if(NodeExists(t_node, "sides")) {
115  CVector2 cFirstPoint, cLastPoint, cCurPoint;
116  std::string strConnectWith;
117  TConfigurationNode& tNode = GetNode(t_node, "sides");
118  TConfigurationNodeIterator tVertexIt("vertex");
119  /* Get the first vertex */
120  tVertexIt = tVertexIt.begin(&tNode);
121  if(tVertexIt == tVertexIt.end()) {
122  THROW_ARGOSEXCEPTION("No <vertex> specified within <sides> section");
123  }
124  GetNodeAttribute(*tVertexIt, "point", cFirstPoint);
125  cLastPoint = cFirstPoint;
126  /* Go through the other vertices */
127  ++tVertexIt;
128  while(tVertexIt != tVertexIt.end()) {
129  /* Read vertex data and fill in segment struct */
130  GetNodeAttribute(*tVertexIt, "point", cCurPoint);
131  SVerticalFace* psFace = new SVerticalFace;
132  psFace->BaseSegment.SetStart(cLastPoint);
133  psFace->BaseSegment.SetEnd(cCurPoint);
134  SideFaces.push_back(psFace);
135  /* Next vertex */
136  cLastPoint = cCurPoint;
137  ++tVertexIt;
138  }
139  /* Make sure that the boundary is a closed path */
140  if(SideFaces.size() < 3) {
141  THROW_ARGOSEXCEPTION("The <sides> path is not closed; at least 3 segments must be specified");
142  }
143  if(cLastPoint != cFirstPoint) {
144  SVerticalFace* psFace = new SVerticalFace;
145  psFace->BaseSegment.SetStart(cLastPoint);
146  psFace->BaseSegment.SetEnd(cFirstPoint);
147  SideFaces.push_back(psFace);
148  }
149  }
150  }
151  catch(CARGoSException& ex) {
152  THROW_ARGOSEXCEPTION_NESTED("Error parsing physics engine <boundaries> information", ex);
153  }
154  }
155 
156  /****************************************/
157  /****************************************/
158 
160  if(TopFace) delete TopFace;
161  if(BottomFace) delete BottomFace;
162  while(!SideFaces.empty()) {
163  delete SideFaces.back();
164  SideFaces.pop_back();
165  }
166  }
167 
168  /****************************************/
169  /****************************************/
170 
172  return TopFace || BottomFace || (!SideFaces.empty());
173  }
174 
175  /****************************************/
176  /****************************************/
177 
179  m_unIterations(10),
180  m_fPhysicsClockTick(m_fSimulationClockTick) {}
181 
182  /****************************************/
183  /****************************************/
184 
186  try {
187  /* Get id from the XML */
188  GetNodeAttribute(t_tree, "id", m_strId);
189  /* Get iterations per time step */
190  GetNodeAttributeOrDefault(t_tree, "iterations", m_unIterations, m_unIterations);
191  m_fPhysicsClockTick = GetSimulationClockTick() / static_cast<Real>(m_unIterations);
192  LOG << "[INFO] The physics engine \""
193  << GetId()
194  << "\" will perform "
195  << m_unIterations
196  << " iterations per tick (dt = "
197  << GetPhysicsClockTick() << " sec)"
198  << std::endl;
199  /* Parse the boundary definition, if necessary */
200  if(NodeExists(t_tree, "boundaries")) {
201  m_sVolume.Init(GetNode(t_tree, "boundaries"));
202  }
203  }
204  catch(CARGoSException& ex) {
205  THROW_ARGOSEXCEPTION("Error initializing a physics engine");
206  }
207  }
208 
209  /****************************************/
210  /****************************************/
211 
213  if(! IsEntityTransferActive()) {
214  /*
215  * The engine has no boundaries, so the wanted point is in for sure
216  */
217  return true;
218  }
219  else {
220  /*
221  * Check the boundaries
222  */
223  /* Check top/bottom boundaries */
224  if((m_sVolume.TopFace && c_point.GetZ() > m_sVolume.TopFace->Height) ||
225  (m_sVolume.BottomFace && c_point.GetZ() < m_sVolume.BottomFace->Height)) {
226  return false;
227  }
228  /* Check side boundaries */
229  for(size_t i = 0; i < GetVolume().SideFaces.size(); ++i) {
230  const CVector2& cP0 = GetVolume().SideFaces[i]->BaseSegment.GetStart();
231  const CVector2& cP1 = GetVolume().SideFaces[i]->BaseSegment.GetEnd();
232  Real fCriterion =
233  (c_point.GetY() - cP0.GetY()) * (cP1.GetX() - cP0.GetX()) -
234  (c_point.GetX() - cP0.GetX()) * (cP1.GetY() - cP0.GetY());
235  if(fCriterion < 0.0f) {
236  return false;
237  }
238  }
239  return true;
240  }
241  }
242 
243  /****************************************/
244  /****************************************/
245 
247  m_vecTransferData.push_back(&c_entity);
248  }
249 
250  /****************************************/
251  /****************************************/
252 
254  for(size_t i = 0; i < m_vecTransferData.size(); ++i) {
255  RemoveEntity(m_vecTransferData[i]->GetRootEntity());
256  CSimulator::GetInstance().GetSpace().AddEntityToPhysicsEngine(*m_vecTransferData[i]);
257  }
258  m_vecTransferData.clear();
259  }
260 
261  /****************************************/
262  /****************************************/
263 
265  return m_fSimulationClockTick;
266  }
267 
268  /****************************************/
269  /****************************************/
270 
272  return m_fInverseSimulationClockTick;
273  }
274 
275  /****************************************/
276  /****************************************/
277 
278  void CPhysicsEngine::SetSimulationClockTick(Real f_simulation_clock_tick) {
279  LOG << "[INFO] Using simulation clock tick = " << f_simulation_clock_tick << std::endl;
280  m_fSimulationClockTick = f_simulation_clock_tick;
281  m_fInverseSimulationClockTick = 1.0f / f_simulation_clock_tick;
282  }
283 
284  /****************************************/
285  /****************************************/
286 
287 }
A 3D vector class.
Definition: vector3.h:29
CPhysicsEngine::TVector & GetPhysicsEngines()
Returns the list of currently existing physics engines.
Definition: simulator.h:119
void GetNodeAttributeOrDefault(TConfigurationNode &t_node, const std::string &str_attribute, T &t_buffer, const T &t_default)
Returns the value of a node's attribute, or the passed default value.
SVolume & GetVolume()
Returns the boundary faces for the volume associated to this engine.
static Real GetInverseSimulationClockTick()
Returns the inverse of GetSimulationClockTick().
float Real
Collects all ARGoS code.
Definition: datatypes.h:39
CARGoSLog LOG(std::cout, SLogColor(ARGOS_LOG_ATTRIBUTE_BRIGHT, ARGOS_LOG_COLOR_GREEN))
Definition: argos_log.h:179
#define THROW_ARGOSEXCEPTION(message)
This macro throws an ARGoS exception with the passed message.
Real GetX() const
Returns the x coordinate of this vector.
Definition: vector2.h:78
Real GetX() const
Returns the x coordinate of this vector.
Definition: vector3.h:93
virtual bool IsPointContained(const CVector3 &c_point)
Returns true if the given point is contained in this physics engine.
Real GetY() const
Returns the y coordinate of this vector.
Definition: vector3.h:109
Real GetPhysicsClockTick() const
Returns the length of the physics engine tick.
TConfigurationNode & GetNode(TConfigurationNode &t_node, const std::string &str_tag)
Given a tree root node, returns the first of its child nodes with the wanted name.
virtual void Init(TConfigurationNode &t_tree)
Initializes the resource.
ticpp::Element TConfigurationNode
The ARGoS configuration XML node.
Real GetY() const
Returns the y coordinate of this vector.
Definition: vector2.h:94
This entity is a link to a body in the physics engine.
#define THROW_ARGOSEXCEPTION_NESTED(message, nested)
This macro throws an ARGoS exception with the passed message and nesting the passed exception...
void Init(TConfigurationNode &t_node)
void SetStart(const CVector2 &c_start)
Definition: ray2.h:44
const std::string & GetId() const
Returns the id of this physics engine.
virtual bool RemoveEntity(CEntity &c_entity)=0
Removes an entity from the physics engine.
virtual void TransferEntities()
Executes the transfer of entities to other engines.
bool GetEmbodiedEntitiesIntersectedByRay(TEmbodiedEntityIntersectionData &t_data, const CRay3 &c_ray)
Checks whether the given ray intersects any entity.
std::vector< SVerticalFace * > SideFaces
std::vector< CPhysicsEngine * > TVector
void SetEnd(const CVector2 &c_end)
Definition: ray2.h:48
std::vector< SEmbodiedEntityIntersectionItem > TEmbodiedEntityIntersectionData
bool IsEntityTransferActive() const
Returns true if entity transfer is active for this engine.
A boundary face for top/bottom parts of the volume.
A 2D vector class.
Definition: vector2.h:25
bool NodeExists(TConfigurationNode &t_node, const std::string &str_tag)
Given a tree root node, returns true if one of its child nodes has the wanted name.
ticpp::Iterator< ticpp::Element > TConfigurationNodeIterator
The iterator for the ARGoS configuration XML node.
void GetNodeAttribute(TConfigurationNode &t_node, const std::string &str_attribute, T &t_buffer)
Returns the value of a node's attribute.
The exception that wraps all errors in ARGoS.
bool GetClosestEmbodiedEntityIntersectedByRay(SEmbodiedEntityIntersectionItem &s_item, const CRay3 &c_ray)
Returns the closest intersection with an embodied entity to the ray start.
static Real GetSimulationClockTick()
Returns the simulation clock tick.
virtual void AddEntityToPhysicsEngine(CEmbodiedEntity &c_entity)
Definition: space.cpp:163
A boundary face for side parts of the volume.
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
Real GetZ() const
Returns the z coordinate of this vector.
Definition: vector3.h:125
static void SetSimulationClockTick(Real f_simulation_clock_tick)
Sets the simulation clock tick.
virtual void ScheduleEntityForTransfer(CEmbodiedEntity &c_entity)
Schedules an entity of transfer.
The core class of ARGOS.
Definition: simulator.h:62
static CSimulator & GetInstance()
Returns the instance to the CSimulator class.
Definition: simulator.cpp:78
CSpace & GetSpace() const
Returns a reference to the simulated space.
Definition: simulator.h:104