ARGoS  3
A parallel, multi-engine simulator for swarm robotics
plugins/simulator/physics_engines/dynamics2d/dynamics2d_engine.cpp
Go to the documentation of this file.
00001 
00007 #include "dynamics2d_engine.h"
00008 #include "dynamics2d_model.h"
00009 #include "dynamics2d_gripping.h"
00010 
00011 #include <argos3/core/simulator/simulator.h>
00012 
00013 #include <cmath>
00014 
00015 namespace argos {
00016 
00017    /****************************************/
00018    /****************************************/
00019 
00020    CDynamics2DEngine::CDynamics2DEngine() :
00021       m_fStaticHashCellSize(0.1f),
00022       m_fActiveHashCellSize(2.0f * 0.085036758f),
00023       m_nStaticHashCells(1000),
00024       m_nActiveHashCells(1000),
00025       m_ptSpace(NULL),
00026       m_ptGroundBody(NULL),
00027       m_fElevation(0.0f),
00028       m_bEntityTransferActive(false) {
00029    }
00030 
00031    /****************************************/
00032    /****************************************/
00033 
00034    void CDynamics2DEngine::Init(TConfigurationNode& t_tree) {
00035       /* Init parent */
00036       CPhysicsEngine::Init(t_tree);
00037       /* Parse XML */
00038       GetNodeAttributeOrDefault(t_tree, "static_cell_size", m_fStaticHashCellSize, m_fStaticHashCellSize);
00039       GetNodeAttributeOrDefault(t_tree, "active_cell_size", m_fActiveHashCellSize, m_fActiveHashCellSize);
00040       GetNodeAttributeOrDefault(t_tree, "static_cells",     m_nStaticHashCells,    m_nStaticHashCells);
00041       GetNodeAttributeOrDefault(t_tree, "active_cells",     m_nActiveHashCells,    m_nActiveHashCells);
00042       GetNodeAttributeOrDefault(t_tree, "elevation",        m_fElevation,          m_fElevation);
00043       if(NodeExists(t_tree, "boundaries")) {
00044          /* Parse the boundary definition */
00045          TConfigurationNode& tBoundaries = GetNode(t_tree, "boundaries");
00046          SBoundarySegment sBoundSegment;
00047          CVector2 cLastPoint, cCurPoint;
00048          std::string strConnectWith;
00049          TConfigurationNodeIterator tVertexIt("vertex");
00050          /* Get the first vertex */
00051          tVertexIt = tVertexIt.begin(&tBoundaries);
00052          if(tVertexIt == tVertexIt.end()) {
00053             THROW_ARGOSEXCEPTION("Physics engine of type \"dynamics2d\", id \"" << GetId() << "\": you didn't specify any <vertex>!");
00054          }
00055          GetNodeAttribute(*tVertexIt, "point", cLastPoint);
00056          m_vecVertices.push_back(cLastPoint);
00057          /* Go through the other vertices */
00058          ++tVertexIt;
00059          while(tVertexIt != tVertexIt.end()) {
00060             /* Read vertex data and fill in segment struct */
00061             GetNodeAttribute(*tVertexIt, "point", cCurPoint);
00062             m_vecVertices.push_back(cCurPoint);
00063             sBoundSegment.Segment.SetStart(cLastPoint);
00064             sBoundSegment.Segment.SetEnd(cCurPoint);
00065             GetNodeAttribute(*tVertexIt, "connect_with", strConnectWith);
00066             if(strConnectWith == "gate") {
00067                /* Connect to previous vertex with a gate */
00068                sBoundSegment.Type = SBoundarySegment::SEGMENT_TYPE_GATE;
00069                GetNodeAttribute(*tVertexIt, "to_engine", sBoundSegment.EngineId);
00070             }
00071             else if(strConnectWith == "wall") {
00072                /* Connect to previous vertex with a wall */
00073                sBoundSegment.Type = SBoundarySegment::SEGMENT_TYPE_WALL;
00074                sBoundSegment.EngineId = "";
00075             }
00076             else {
00077                /* Parse error */
00078                THROW_ARGOSEXCEPTION("Physics engine of type \"dynamics2d\", id \"" << GetId() << "\": unknown vertex connection method \"" << strConnectWith << "\". Allowed methods are \"wall\" and \"gate\".");
00079             }
00080             m_vecSegments.push_back(sBoundSegment);
00081             /* Next vertex */
00082             cLastPoint = cCurPoint;
00083             ++tVertexIt;
00084          }
00085          /* Check that the boundary is a closed path */
00086          if(m_vecVertices.front() != m_vecVertices.back()) {
00087             THROW_ARGOSEXCEPTION("Physics engine of type \"dynamics2d\", id \"" << GetId() << "\": the specified path is not closed. The first and last points of the boundaries MUST be the same.");
00088          }
00089       }
00090       /* Initialize physics */
00091       cpInitChipmunk();
00092       cpResetShapeIdCounter();
00093       /* Used to attach static geometries so that they won't move and to simulate friction */
00094       m_ptGroundBody = cpBodyNew(INFINITY, INFINITY);
00095       /* Create the space to contain the movable objects */
00096       m_ptSpace = cpSpaceNew();
00097       /* Subiterations to solve constraints.
00098          The more, the better for precision but the worse for speed
00099       */
00100       m_ptSpace->iterations = GetIterations();
00101       /* Resize the space hash.
00102          This has dramatic effects on performance.
00103          TODO: - find optimal parameters automatically (average entity size)
00104       cpSpaceReindexStaticHash(m_ptSpace, m_fStaticHashCellSize, m_nStaticHashCells);
00105       cpSpaceResizeActiveHash(m_ptSpace, m_fActiveHashCellSize, m_nActiveHashCells);
00106       */
00107       /* Gripper-Gripped callback functions */
00108       cpSpaceAddCollisionHandler(
00109          m_ptSpace,
00110          SHAPE_GRIPPER,
00111          SHAPE_GRIPPABLE,
00112          BeginCollisionBetweenGripperAndGrippable,
00113          ManageCollisionBetweenGripperAndGrippable,
00114          NULL,
00115          NULL,
00116          NULL);
00117       /* Add boundaries, if specified */
00118       if(! m_vecSegments.empty()) {
00119          cpShape* ptSegment;
00120          for(size_t i = 0; i < m_vecSegments.size(); ++i) {
00121             if(m_vecSegments[i].Type == SBoundarySegment::SEGMENT_TYPE_WALL) {
00122                ptSegment =
00123                   cpSpaceAddShape(
00124                      m_ptSpace,
00125                      cpSegmentShapeNew(
00126                         m_ptGroundBody,
00127                         cpv(m_vecSegments[i].Segment.GetStart().GetX(),
00128                             m_vecSegments[i].Segment.GetStart().GetY()),
00129                         cpv(m_vecSegments[i].Segment.GetEnd().GetX(),
00130                             m_vecSegments[i].Segment.GetEnd().GetY()),
00131                         0.0f));
00132                ptSegment->e = 0.0f; // no elasticity
00133                ptSegment->u = 1.0f; // max friction
00134             }
00135             else {
00136                /* There is at least a gate, transfer is activated */
00137                m_bEntityTransferActive = true;
00138             }
00139          }
00140       }
00141    }
00142 
00143    /****************************************/
00144    /****************************************/
00145 
00146    void CDynamics2DEngine::Reset() {
00147       for(CDynamics2DModel::TMap::iterator it = m_tPhysicsModels.begin();
00148           it != m_tPhysicsModels.end(); ++it) {
00149          it->second->Reset();
00150       }
00151       cpSpaceReindexStatic(m_ptSpace);
00152    }
00153 
00154    /****************************************/
00155    /****************************************/
00156 
00157    void CDynamics2DEngine::Update() {
00158       /* Update the physics state from the entities */
00159       for(CDynamics2DModel::TMap::iterator it = m_tPhysicsModels.begin();
00160           it != m_tPhysicsModels.end(); ++it) {
00161          it->second->UpdateFromEntityStatus();
00162       }
00163       /* Perform the step */
00164       cpSpaceStep(m_ptSpace, GetSimulationClockTick());
00165       /* Update the simulated space */
00166       for(CDynamics2DModel::TMap::iterator it = m_tPhysicsModels.begin();
00167           it != m_tPhysicsModels.end(); ++it) {
00168          it->second->UpdateEntityStatus();
00169       }
00170    }
00171 
00172    /****************************************/
00173    /****************************************/
00174 
00175    void CDynamics2DEngine::Destroy() {
00176       /* Empty the physics model map */
00177       for(CDynamics2DModel::TMap::iterator it = m_tPhysicsModels.begin();
00178           it != m_tPhysicsModels.end(); ++it) {
00179          delete it->second;
00180       }
00181       m_tPhysicsModels.clear();
00182       /* Get rid of the physics space */
00183       cpSpaceFree(m_ptSpace);
00184       cpBodyFree(m_ptGroundBody);
00185    }
00186 
00187    /****************************************/
00188    /****************************************/
00189 
00190    bool CDynamics2DEngine::IsPointContained(const CVector3& c_point) {
00191       /* Check that Z-coordinate is within elevation */
00192       if(c_point.GetZ() > m_fElevation || c_point.GetZ() < m_fElevation) {
00193          return false;
00194       }
00195       if(! IsEntityTransferActive()) {
00196          /* The engine has no boundaries on XY, so the wanted point is in for sure */
00197          return true;
00198       }
00199       else {
00200          /* Check the boundaries */
00201          for(size_t i = 0; i < m_vecSegments.size(); ++i) {
00202             const CVector2& cP0 = m_vecSegments[i].Segment.GetStart();
00203             const CVector2& cP1 = m_vecSegments[i].Segment.GetEnd();
00204             Real fCriterion =
00205                (c_point.GetY() - cP0.GetY()) * (cP1.GetX() - cP0.GetX()) -
00206                (c_point.GetX() - cP0.GetX()) * (cP1.GetY() - cP0.GetY());
00207             if(fCriterion > 0.0f) {
00208                return false;
00209             }
00210          }
00211          return true;
00212       }
00213    }
00214 
00215    /****************************************/
00216    /****************************************/
00217 
00218    UInt32 CDynamics2DEngine::GetNumPhysicsEngineEntities() {
00219       return m_tPhysicsModels.size();
00220    }
00221 
00222    /****************************************/
00223    /****************************************/
00224 
00225    void CDynamics2DEngine::AddEntity(CEntity& c_entity) {
00226       CallEntityOperation<CDynamics2DOperationAddEntity, CDynamics2DEngine, void>(*this, c_entity);
00227       cpResetShapeIdCounter();
00228    }
00229 
00230    /****************************************/
00231    /****************************************/
00232 
00233    void CDynamics2DEngine::RemoveEntity(CEntity& c_entity) {
00234       CallEntityOperation<CDynamics2DOperationRemoveEntity, CDynamics2DEngine, void>(*this, c_entity);
00235    }
00236 
00237    /****************************************/
00238    /****************************************/
00239 
00240    void CDynamics2DEngine::TransferEntities() {
00241       for(size_t i = 0; i < m_vecTransferData.size(); ++i) {
00242          CPhysicsEngine& cToEngine = CSimulator::GetInstance().GetPhysicsEngine(m_vecTransferData[i].EngineId);
00243          cToEngine.AddEntity(*m_vecTransferData[i].Entity);
00244          RemoveEntity(*m_vecTransferData[i].Entity);
00245       }
00246       m_vecTransferData.clear();
00247    }
00248 
00249    /****************************************/
00250    /****************************************/
00251    
00252    CEmbodiedEntity* CDynamics2DEngine::CheckIntersectionWithRay(Real& f_t_on_ray,
00253                                                                 const CRay3& c_ray) const {
00254       /* Structure to store the query data */
00255       cpSegmentQueryInfo tInfo;
00256       /* Query the first hit along the ray */
00257       cpShape* ptHit = cpSpaceSegmentQueryFirst(
00258          m_ptSpace,
00259          cpv(c_ray.GetStart().GetX(), c_ray.GetStart().GetY()),
00260          cpv(c_ray.GetEnd().GetX()  , c_ray.GetEnd().GetY()  ),
00261          CP_ALL_LAYERS,
00262          CP_NO_GROUP,
00263          &tInfo);
00264       /* Check whether we have a hit */
00265       if(ptHit) {
00266          /* Hit found, is it within the limits? */
00267          CDynamics2DModel& cModel = *reinterpret_cast<CDynamics2DModel*>(ptHit->body->data);
00268          CVector3 cIntersectionPoint;
00269          c_ray.GetPoint(cIntersectionPoint, tInfo.t);
00270          if((cIntersectionPoint.GetZ() >= cModel.GetBoundingBox().MinCorner.GetZ()) &&
00271                         (cIntersectionPoint.GetZ() <= cModel.GetBoundingBox().MaxCorner.GetZ()) ) {
00272             /* Yes, a real hit */
00273             f_t_on_ray = tInfo.t;
00274             return &cModel.GetEmbodiedEntity();
00275          }
00276          else {
00277             return NULL;
00278          }
00279       }
00280       else {
00281          /* Hit not found */
00282          return NULL;
00283       }
00284    }
00285 
00286    /****************************************/
00287    /****************************************/
00288 
00289    bool CDynamics2DEngine::CalculateTransfer(Real f_x, Real f_y,
00290                                              std::string& str_engine_id) {
00291       /*
00292        * TODO: this method makes the assumption that only one gate is trespassed at any time.
00293        * This assumption may be false in some ill-shaped polygons or when the gate isn't just a
00294        * segment, but is a sequence of segments.
00295        */
00296       for(size_t i = 0; i < m_vecSegments.size(); ++i) {
00297          if(m_vecSegments[i].Type == SBoundarySegment::SEGMENT_TYPE_GATE) {
00298             const CVector2& cP0 = m_vecSegments[i].Segment.GetStart();
00299             const CVector2& cP1 = m_vecSegments[i].Segment.GetEnd();
00300             Real fCriterion =
00301                (f_y - cP0.GetY()) * (cP1.GetX() - cP0.GetX()) -
00302                (f_x - cP0.GetX()) * (cP1.GetY() - cP0.GetY());
00303             if(fCriterion < 0.0f) {
00304                str_engine_id = m_vecSegments[i].EngineId;
00305                return true;
00306             }
00307          }
00308       }
00309       return false;
00310    }
00311 
00312    /****************************************/
00313    /****************************************/
00314 
00315    void CDynamics2DEngine::PositionPhysicsToSpace(CVector3& c_new_pos,
00316                                                   const CVector3& c_original_pos,
00317                                                   const cpBody* pt_body) {
00318       c_new_pos.SetX(pt_body->p.x);
00319       c_new_pos.SetY(pt_body->p.y);
00320       c_new_pos.SetZ(c_original_pos.GetZ());
00321    }
00322 
00323    /****************************************/
00324    /****************************************/
00325 
00326    void CDynamics2DEngine::OrientationPhysicsToSpace(CQuaternion& c_new_orient,
00327                                                      cpBody* pt_body) {
00328       c_new_orient.FromAngleAxis(CRadians(pt_body->a), CVector3::Z);
00329    }
00330 
00331    /****************************************/
00332    /****************************************/
00333 
00334    void CDynamics2DEngine::AddPhysicsModel(const std::string& str_id,
00335                                             CDynamics2DModel& c_model) {
00336       m_tPhysicsModels[str_id] = &c_model;
00337    }
00338 
00339    /****************************************/
00340    /****************************************/
00341 
00342    void CDynamics2DEngine::RemovePhysicsModel(const std::string& str_id) {
00343       CDynamics2DModel::TMap::iterator it = m_tPhysicsModels.find(str_id);
00344       if(it != m_tPhysicsModels.end()) {
00345          delete it->second;
00346          m_tPhysicsModels.erase(it);
00347       }
00348       else {
00349          THROW_ARGOSEXCEPTION("Dynamics2D model id \"" << str_id << "\" not found in dynamics 2D engine \"" << GetId() << "\"");
00350       }
00351    }
00352 
00353    /****************************************/
00354    /****************************************/
00355 
00356    REGISTER_PHYSICS_ENGINE(CDynamics2DEngine,
00357                            "dynamics2d",
00358                            "Carlo Pinciroli [ilpincy@gmail.com]",
00359                            "1.0",
00360                            "A 2D dynamics physics engine.",
00361                            "This physics engine is a 2D dynamics engine based on the Chipmunk library\n"
00362                            "(http://code.google.com/p/chipmunk-physics).\n\n"
00363                            "REQUIRED XML CONFIGURATION\n\n"
00364                            "  <physics_engines>\n"
00365                            "    ...\n"
00366                            "    <dynamics2d id=\"dyn2d\" />\n"
00367                            "    ...\n"
00368                            "  </physics_engines>\n\n"
00369                            "The 'id' attribute is necessary and must be unique among the physics engines.\n"
00370                            "It is used in the subsequent section <arena_physics> to assign entities to\n"
00371                            "physics engines. If two engines share the same id, initialization aborts.\n\n"
00372                            "OPTIONAL XML CONFIGURATION\n\n"
00373                            "The plane of the physics engine can be translated on the Z axis, to simulate\n"
00374                            "for example hovering objects, such as flying robots. To translate the plane\n"
00375                            "2m up the Z axis, use the 'elevation' attribute as follows:\n\n"
00376                            "  <physics_engines>\n"
00377                            "    ...\n"
00378                            "    <dynamics2d id=\"dyn2d\"\n"
00379                            "                elevation=\"2.0\" />\n"
00380                            "    ...\n"
00381                            "  </physics_engines>\n\n"
00382                            "When not specified, the elevation is zero, which means that the plane\n"
00383                            "corresponds to the XY plane.\n",
00384                            "Under development"
00385       );
00386 
00387 }