ARGoS
3
A parallel, multi-engine simulator for swarm robotics
|
Go to the documentation of this file.
9 #include <argos3/core/simulator/space/space.h>
10 #include <argos3/core/simulator/simulator.h>
11 #include <argos3/core/utility/string_utilities.h>
12 #include <argos3/core/utility/math/matrix/rotationmatrix3.h>
23 m_psOriginAnchor(NULL) {}
29 const std::string& str_id,
34 m_bMovable(b_movable),
36 m_psOriginAnchor(new
SAnchor(*this,
43 m_cInitOriginPosition(c_position),
44 m_cInitOriginOrientation(c_orientation) {
57 for(std::map<std::string, SAnchor*>::iterator it =
m_mapAnchors.begin();
104 for(std::map<std::string, SAnchor*>::iterator it =
m_mapAnchors.begin();
107 psAnchor = it->second;
108 if(psAnchor->
Index > 0) {
139 c_offset_orientation,
152 std::map<std::string, SAnchor*>::iterator it =
m_mapAnchors.find(str_id);
159 ++(it->second->InUseCount);
161 if(it->second->InUseCount == 1) {
171 if(str_id ==
"origin")
return;
180 --((*it)->InUseCount);
182 if((*it)->InUseCount == 0) {
192 std::map<std::string, SAnchor*>::const_iterator it =
m_mapAnchors.find(str_id);
198 return *(it->second);
206 std::map<std::string, SAnchor*>::iterator it =
m_mapAnchors.find(str_id);
212 return *(it->second);
220 std::map<std::string, SAnchor*>::const_iterator it =
m_mapAnchors.find(str_id);
226 return (it->second->InUseCount > 0);
304 return *(it->second);
315 return *(it->second);
335 if(bNoCollision && !b_check_only) {
350 bool bNoCollision =
true;
355 if(bNoCollision && !b_check_only) {
363 for(
size_t j = 0; j < i; ++j) {
375 #define CHECK_CORNER(MINMAX, COORD, OP) \
376 if(m_sBoundingBox->MINMAX ## Corner.Get ## COORD() OP sBBox.MINMAX ## Corner.Get ## COORD()) { \
377 m_sBoundingBox->MINMAX ## Corner.Set ## COORD(sBBox.MINMAX ## Corner.Get ## COORD()); \
426 "\", but this entity has not been added to any physics engine.");
449 const std::string& str_id) {
450 return (ps_anchor->
Id == str_id);
456 void CEmbodiedEntitySpaceHashUpdater::operator()(CAbstractSpaceHash<CEmbodiedEntity>& c_space_hash,
457 CEmbodiedEntity& c_element) {
459 c_space_hash.SpaceToHashTable(m_nMinX, m_nMinY, m_nMinZ, c_element.GetBoundingBox().MinCorner);
461 c_space_hash.SpaceToHashTable(m_nMaxX, m_nMaxY, m_nMaxZ, c_element.GetBoundingBox().MaxCorner);
463 for(
SInt32 nK = m_nMinZ; nK <= m_nMaxZ; ++nK) {
464 for(
SInt32 nJ = m_nMinY; nJ <= m_nMaxY; ++nJ) {
465 for(
SInt32 nI = m_nMinX; nI <= m_nMaxX; ++nI) {
466 c_space_hash.UpdateCell(nI, nJ, nK, c_element);
482 m_cGrid.ClampCoordinates(m_nMinI, m_nMinJ, m_nMinK);
485 m_cGrid.ClampCoordinates(m_nMaxI, m_nMaxJ, m_nMaxK);
487 for(
SInt32 m_nK = m_nMinK; m_nK <= m_nMaxK; ++m_nK) {
488 for(
SInt32 m_nJ = m_nMinJ; m_nJ <= m_nMaxJ; ++m_nJ) {
489 for(
SInt32 m_nI = m_nMinI; m_nI <= m_nMaxI; ++m_nI) {
490 m_cGrid.UpdateCell(m_nI, m_nJ, m_nK, c_entity);
519 class CSpaceOperationRemoveEmbodiedEntity :
public CSpaceOperationRemoveEntity {
521 void ApplyTo(CSpace& c_space, CEmbodiedEntity& c_entity) {
523 CEntity* pcRoot = &c_entity;
524 while(pcRoot->HasParent()) {
525 pcRoot = &pcRoot->GetParent();
529 while(c_entity.GetPhysicsModelsNum() > 0) {
530 c_entity.GetPhysicsModel(0).GetEngine().RemoveEntity(*pcRoot);
533 catch(CARGoSException& ex) {
547 c_space.RemoveEntity(c_entity);
CQuaternion Orientation
The orientation of the anchor wrt the global coordinate system.
CPhysicsModel::TMap m_tPhysicsModelMap
bool IsAnchorEnabled(const std::string &str_id)
Returns true if the given anchor is enabled, false otherwise.
#define CHECK_CORNER(MINMAX, COORD, OP)
const std::string & GetId() const
Returns the id of this entity.
The namespace containing all the ARGoS related code.
T Min(const T &t_v1, const T &t_v2)
Returns the smaller of the two passed arguments.
REGISTER_SPACE_OPERATION(CSpaceOperationAddEntity, CSpaceOperationAddCFloorEntity, CFloorEntity)
const SAnchor & GetAnchor(const std::string &str_id) const
Returns the wanted anchor as a const reference.
Basic class for an entity that contains other entities.
virtual void AddPhysicsModel(const std::string &str_engine_id, CPhysicsModel &c_physics_model)
Adds a new physics model to this embodied entity.
The exception that wraps all errors in ARGoS.
void EnableAnchor(const std::string &str_id)
Enables an anchor.
CQuaternion m_cInitOriginOrientation
This entity is a link to a body in the physics engine.
virtual bool MoveTo(const CVector3 &c_position, const CQuaternion &c_orientation, bool b_check_only=false)
Moves the entity to the wanted position and orientation.
ticpp::Element TConfigurationNode
The ARGoS configuration XML node.
SAnchor * m_psOriginAnchor
CQuaternion OffsetOrientation
The initial orientation of the anchor wrt the body coordinate system.
virtual bool IsCollidingWithSomething() const
Returns true if this entity is colliding with another object.
CPhysicsModel::TVector m_tPhysicsModelVector
CEmbodiedEntityGridUpdater(CGrid< CEmbodiedEntity > &c_grid)
#define THROW_ARGOSEXCEPTION_NESTED(message, nested)
This macro throws an ARGoS exception with the passed message and nesting the passed exception.
virtual ~CEmbodiedEntity()
Class destructor.
void AddEntity(ENTITY &c_entity)
Adds an entity of the given type.
#define THROW_ARGOSEXCEPTION(message)
This macro throws an ARGoS exception with the passed message.
void CalculateBoundingBox()
Calculates the bounding box of this entity.
An anchor related to the body of an entity.
void DisableAnchor(const std::string &str_id)
Disables an anchor.
virtual bool operator()(CEmbodiedEntity &c_entity)
UInt32 Index
The index of the anchor assigned by the embodied entity.
signed int SInt32
32-bit signed integer.
CEmbodiedEntity(CComposableEntity *pc_parent)
Class constructor.
bool operator==(const SAnchor *ps_anchor, const std::string &str_id)
Returns true if the anchor id matches the given id.
std::string Id
The id of the anchor.
CVector3 m_cInitOriginPosition
void RemovePhysicsModel(const std::string &str_engine_id)
Removes a physics model from this embodied entity.
const CPhysicsModel & GetPhysicsModel(size_t un_idx) const
Returns a physics model associated to this entity.
std::map< std::string, SAnchor * > m_mapAnchors
unsigned int UInt32
32-bit unsigned integer.
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.
UInt32 GetPhysicsModelsNum() const
Returns the number of physics models associated to this entity.
SBoundingBox * m_sBoundingBox
std::string GetContext() const
Returns the context of this entity.
virtual void Init(TConfigurationNode &t_tree)
Initializes the state of the entity from the XML configuration tree.
SAnchor & AddAnchor(const std::string &str_id, const CVector3 &c_rel_position=CVector3(), const CQuaternion &c_rel_orientation=CQuaternion())
Adds an anchor to the embodied entity.
std::vector< SAnchor * > m_vecEnabledAnchors
CVector3 & Rotate(const CQuaternion &c_quaternion)
Rotates this vector by the given quaternion.
virtual void AddEntityToPhysicsEngine(CEmbodiedEntity &c_entity)
virtual void Reset()
Resets the state of the entity to whatever it was after Init() or the standalone constructor was call...
CVector3 Position
The position of the anchor wrt the global coordinate system.
virtual void Init(TConfigurationNode &t_tree)
Initializes the state of the entity from the XML configuration tree.
T Max(const T &t_v1, const T &t_v2)
Returns the bigger of the two passed arguments.
const SBoundingBox & GetBoundingBox() const
Returns the bounding box of this embodied entity.
CVector3 OffsetPosition
The initial position of the anchor wrt the body coordinate system.