ARGoS  3
A parallel, multi-engine simulator for swarm robotics
plugins/simulator/physics_engines/dynamics2d/dynamics2d_engine.h
Go to the documentation of this file.
00001 
00007 #ifndef DYNAMICS2D_ENGINE_H
00008 #define DYNAMICS2D_ENGINE_H
00009 
00010 namespace argos {
00011    class CDynamics2DEngine;
00012    class CDynamics2DModel;
00013    class CGripperEquippedEntity;
00014    class CEmbodiedEntity;
00015 }
00016 
00017 #include <argos3/core/utility/math/ray2.h>
00018 #include <argos3/core/simulator/entity/controllable_entity.h>
00019 #include <argos3/core/simulator/physics_engine/physics_engine.h>
00020 #include <argos3/plugins/simulator/physics_engines/dynamics2d/chipmunk-physics/include/chipmunk.h>
00021 
00022 namespace argos {
00023 
00024    /****************************************/
00025    /****************************************/
00026 
00027    struct SDynamics2DEngineGripperData {
00028       CDynamics2DEngine& Engine;
00029       CGripperEquippedEntity& GripperEntity;
00030       cpVect GripperAnchor;
00031       cpConstraint* GripConstraint;
00032 
00033       SDynamics2DEngineGripperData(CDynamics2DEngine& c_engine,
00034                                    CGripperEquippedEntity& c_entity,
00035                                    cpVect t_anchor);
00036       ~SDynamics2DEngineGripperData();
00037       void ClearConstraints();
00038    };
00039 
00040    /****************************************/
00041    /****************************************/
00042 
00043    class CDynamics2DEngine : public CPhysicsEngine {
00044 
00045    public:
00046 
00047       enum EShapeType {
00048          SHAPE_NORMAL = 0,
00049          SHAPE_GRIPPABLE,
00050          SHAPE_GRIPPER
00051       };
00052 
00053       enum ELayerType {
00054          LAYER_FROZEN = 0,
00055          LAYER_NORMAL = CP_ALL_LAYERS
00056       };
00057 
00058       struct SBoundarySegment {
00059          CRay2 Segment;
00060          enum {
00061             SEGMENT_TYPE_WALL,
00062             SEGMENT_TYPE_GATE
00063          } Type;
00064          std::string EngineId;
00065 
00066          SBoundarySegment() : 
00067             Type(SEGMENT_TYPE_WALL) {}
00068       };
00069 
00070       struct SEntityTransferData {
00071          std::string EngineId;
00072          CEntity* Entity;
00073 
00074          SEntityTransferData() :
00075             Entity(NULL) {}
00076       };
00077 
00078       CDynamics2DEngine();
00079 
00080       virtual ~CDynamics2DEngine() {}
00081 
00082       virtual void Init(TConfigurationNode& t_tree);
00083 
00084       virtual void Reset();
00085 
00086       virtual void Update();
00087 
00088       virtual void Destroy();
00089 
00090       virtual bool IsPointContained(const CVector3& c_point);
00091 
00092       virtual UInt32 GetNumPhysicsEngineEntities();
00093 
00094       virtual void AddEntity(CEntity& c_entity);
00095 
00096       virtual void RemoveEntity(CEntity& c_entity);
00097 
00098       inline virtual bool IsEntityTransferNeeded() const {
00099          return ! m_vecTransferData.empty();
00100       }
00101 
00102       virtual void TransferEntities();
00103 
00104       inline virtual bool IsEntityTransferActive() const {
00105          return m_bEntityTransferActive;
00106       }
00107 
00108       virtual CEmbodiedEntity* CheckIntersectionWithRay(Real& f_t_on_ray,
00109                                                         const CRay3& c_ray) const;
00110 
00111       bool CalculateTransfer(Real f_x, Real f_y,
00112                              std::string& str_engine_id);
00113 
00114       inline void ScheduleEntityForTransfer(CEntity& c_entity,
00115                                             const std::string& str_engine_id) {
00116          m_vecTransferData.push_back(SEntityTransferData());
00117          m_vecTransferData.back().EngineId = str_engine_id;
00118          m_vecTransferData.back().Entity = &c_entity;
00119       }
00120 
00121       inline cpSpace* GetPhysicsSpace() {
00122          return m_ptSpace;
00123       }
00124 
00125       inline cpBody* GetGroundBody() {
00126          return m_ptGroundBody;
00127       }
00128 
00129       inline Real GetElevation() const {
00130          return m_fElevation;
00131       }
00132 
00133       inline Real GetDamping() const {
00134          return cpSpaceGetDamping(m_ptSpace);
00135       }
00136 
00137       inline void SetDamping(Real f_damping) {
00138          cpSpaceSetDamping(m_ptSpace, f_damping);
00139       }
00140 
00141       inline CVector2 GetGravity() const {
00142          return CVector2(m_ptSpace->gravity.x, m_ptSpace->gravity.y);
00143       }
00144 
00145       inline void SetGravity(const CVector2& c_gravity) {
00146          m_ptSpace->gravity = cpv(c_gravity.GetX(), c_gravity.GetY());
00147       }
00148 
00149       void PositionPhysicsToSpace(CVector3& c_new_pos,
00150                                   const CVector3& c_original_pos,
00151                                   const cpBody* pt_body);
00152 
00153       void OrientationPhysicsToSpace(CQuaternion& c_new_orient,
00154                                      cpBody* pt_body);
00155       
00156       void AddPhysicsModel(const std::string& str_id,
00157                             CDynamics2DModel& c_model);
00158       void RemovePhysicsModel(const std::string& str_id);
00159 
00160    private:
00161 
00162       cpFloat m_fStaticHashCellSize;
00163       cpFloat m_fActiveHashCellSize;
00164       SInt32 m_nStaticHashCells;
00165       SInt32 m_nActiveHashCells;
00166       cpSpace* m_ptSpace;
00167       cpBody* m_ptGroundBody;
00168       Real m_fElevation;
00169 
00170       std::vector<CVector2> m_vecVertices;
00171       std::vector<SBoundarySegment> m_vecSegments;
00172       std::vector<SEntityTransferData> m_vecTransferData;
00173       bool m_bEntityTransferActive;
00174 
00175       CControllableEntity::TMap m_tControllableEntities;
00176       std::map<std::string, CDynamics2DModel*> m_tPhysicsModels;
00177 
00178    };
00179 
00180    /****************************************/
00181    /****************************************/
00182 
00183    template <typename ACTION>
00184    class CDynamics2DOperation : public CEntityOperation<ACTION, CDynamics2DEngine, void> {
00185    public:
00186       virtual ~CDynamics2DOperation() {}
00187    };
00188 
00189    class CDynamics2DOperationAddEntity : public CDynamics2DOperation<CDynamics2DOperationAddEntity> {
00190    public:
00191       virtual ~CDynamics2DOperationAddEntity() {}
00192    };
00193 
00194    class CDynamics2DOperationRemoveEntity : public CDynamics2DOperation<CDynamics2DOperationRemoveEntity> {
00195    public:
00196       virtual ~CDynamics2DOperationRemoveEntity() {}
00197    };
00198 
00199 #define REGISTER_DYNAMICS2D_OPERATION(ACTION, OPERATION, ENTITY)        \
00200    REGISTER_ENTITY_OPERATION(ACTION, CDynamics2DEngine, OPERATION, void, ENTITY);
00201 
00202 #define REGISTER_STANDARD_DYNAMICS2D_OPERATION_ADD_ENTITY(SPACE_ENTITY, DYN2D_MODEL) \
00203    class CDynamics2DOperationAdd ## SPACE_ENTITY : public CDynamics2DOperationAddEntity { \
00204    public:                                                              \
00205    CDynamics2DOperationAdd ## SPACE_ENTITY() {}                         \
00206    virtual ~CDynamics2DOperationAdd ## SPACE_ENTITY() {}                \
00207    void ApplyTo(CDynamics2DEngine& c_engine,                            \
00208                 SPACE_ENTITY& c_entity) {                               \
00209       DYN2D_MODEL* pcPhysModel = new DYN2D_MODEL(c_engine,              \
00210                                                  c_entity);             \
00211       c_engine.AddPhysicsModel(c_entity.GetId(),                        \
00212                                *pcPhysModel);                           \
00213       c_entity.                                                         \
00214          GetComponent<CEmbodiedEntity>("body").                         \
00215          AddPhysicsModel(c_engine.GetId(), *pcPhysModel);               \
00216    }                                                                    \
00217    };                                                                   \
00218    REGISTER_DYNAMICS2D_OPERATION(CDynamics2DOperationAddEntity,         \
00219                                  CDynamics2DOperationAdd ## SPACE_ENTITY, \
00220                                  SPACE_ENTITY);
00221    
00222 #define REGISTER_STANDARD_DYNAMICS2D_OPERATION_REMOVE_ENTITY(SPACE_ENTITY) \
00223    class CDynamics2DOperationRemove ## SPACE_ENTITY : public CDynamics2DOperationRemoveEntity { \
00224    public:                                                              \
00225    CDynamics2DOperationRemove ## SPACE_ENTITY() {}                      \
00226    virtual ~CDynamics2DOperationRemove ## SPACE_ENTITY() {}             \
00227    void ApplyTo(CDynamics2DEngine& c_engine,                            \
00228                 SPACE_ENTITY& c_entity) {                               \
00229       c_engine.RemovePhysicsModel(c_entity.GetId());                    \
00230       c_entity.                                                         \
00231          GetComponent<CEmbodiedEntity>("body").                         \
00232          RemovePhysicsModel(c_engine.GetId());                          \
00233    }                                                                    \
00234    };                                                                   \
00235    REGISTER_DYNAMICS2D_OPERATION(CDynamics2DOperationRemoveEntity,      \
00236                                  CDynamics2DOperationRemove ## SPACE_ENTITY, \
00237                                  SPACE_ENTITY);
00238    
00239 #define REGISTER_STANDARD_DYNAMICS2D_OPERATIONS_ON_ENTITY(SPACE_ENTITY, DYN2D_ENTITY) \
00240    REGISTER_STANDARD_DYNAMICS2D_OPERATION_ADD_ENTITY(SPACE_ENTITY, DYN2D_ENTITY) \
00241    REGISTER_STANDARD_DYNAMICS2D_OPERATION_REMOVE_ENTITY(SPACE_ENTITY)
00242 
00243    /****************************************/
00244    /****************************************/
00245 
00246 }
00247 
00248 #endif