ARGoS
3
A parallel, multi-engine simulator for swarm robotics
|
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