ARGoS  3
A parallel, multi-engine simulator for swarm robotics
plugins/simulator/physics_engines/dynamics2d/dynamics2d_box_model.cpp
Go to the documentation of this file.
00001 
00007 #include "dynamics2d_box_model.h"
00008 #include "dynamics2d_gripping.h"
00009 #include "dynamics2d_engine.h"
00010 
00011 namespace argos {
00012 
00013    /****************************************/
00014    /****************************************/
00015 
00016    CDynamics2DBoxModel::CDynamics2DBoxModel(CDynamics2DEngine& c_engine,
00017                                             CBoxEntity& c_entity) :
00018       CDynamics2DModel(c_engine, c_entity.GetEmbodiedEntity()),
00019       m_bMovable(false),
00020       m_cBoxEntity(c_entity),
00021       m_pcGrippable(NULL),
00022       m_fMass(c_entity.GetMass()),
00023       m_ptShape(NULL),
00024       m_ptBody(NULL) {
00025       /* Set movable flag */
00026       m_bMovable = c_entity.GetEmbodiedEntity().IsMovable();
00027       /* Get the size of the entity */
00028       CVector3 cHalfSize = c_entity.GetSize() * 0.5f;
00029       /* Create a polygonal object in the physics space */
00030       /* Start defining the vertices
00031          NOTE: points must be defined in a clockwise winding
00032       */
00033       cpVect tVertices[] = {
00034          cpv(-cHalfSize.GetX(), -cHalfSize.GetY()),
00035          cpv(-cHalfSize.GetX(),  cHalfSize.GetY()),
00036          cpv( cHalfSize.GetX(),  cHalfSize.GetY()),
00037          cpv( cHalfSize.GetX(), -cHalfSize.GetY())
00038       };
00039       const CVector3& cPosition = GetEmbodiedEntity().GetPosition();
00040       CRadians cXAngle, cYAngle, cZAngle;
00041       GetEmbodiedEntity().GetOrientation().ToEulerAngles(cZAngle, cYAngle, cXAngle);
00042       if(m_bMovable) {
00043          /* The box is movable */
00044          /* Create the body */
00045          m_ptBody =
00046             cpSpaceAddBody(m_cDyn2DEngine.GetPhysicsSpace(),
00047                            cpBodyNew(m_fMass,
00048                                      cpMomentForPoly(m_fMass,
00049                                                      4,
00050                                                      tVertices,
00051                                                      cpvzero)));
00052          m_ptBody->p = cpv(cPosition.GetX(), cPosition.GetY());
00053          cpBodySetAngle(m_ptBody, cZAngle.GetValue());
00054          /* Create the shape */
00055          m_ptShape =
00056             cpSpaceAddShape(m_cDyn2DEngine.GetPhysicsSpace(),
00057                             cpPolyShapeNew(m_ptBody,
00058                                            4,
00059                                            tVertices,
00060                                            cpvzero));
00061          m_ptShape->e = 0.0; // no elasticity
00062          m_ptShape->u = 0.7; // lots contact friction to help pushing
00063          /* The shape is grippable */
00064          m_pcGrippable = new CDynamics2DGrippable(GetEmbodiedEntity(),
00065                                                   m_ptShape);
00066          /* Friction with ground */
00067          m_ptLinearFriction =
00068             cpSpaceAddConstraint(m_cDyn2DEngine.GetPhysicsSpace(),
00069                                  cpPivotJointNew2(m_cDyn2DEngine.GetGroundBody(),
00070                                                   m_ptBody,
00071                                                   cpvzero,
00072                                                   cpvzero));
00073          m_ptLinearFriction->maxBias = 0.0f; // disable joint correction
00074          m_ptLinearFriction->maxForce = 1.49f; // emulate linear friction (this is just slightly smaller than FOOTBOT_MAX_FORCE)
00075          m_ptAngularFriction =
00076             cpSpaceAddConstraint(m_cDyn2DEngine.GetPhysicsSpace(),
00077                                  cpGearJointNew(m_cDyn2DEngine.GetGroundBody(),
00078                                                 m_ptBody,
00079                                                 0.0f,
00080                                                 1.0f));
00081          m_ptAngularFriction->maxBias = 0.0f; // disable joint correction
00082          m_ptAngularFriction->maxForce = 1.49f; // emulate angular friction (this is just slightly smaller than FOOTBOT_MAX_TORQUE)
00083       }
00084       else {
00085          /* The box is not movable */
00086          /* Create a static body */
00087          m_ptBody = cpBodyNewStatic();
00088          /* Manually rotate the vertices */
00089          cpVect tRot = cpvforangle(cZAngle.GetValue());
00090          tVertices[0] = cpvrotate(tVertices[0], tRot);
00091          tVertices[1] = cpvrotate(tVertices[1], tRot);
00092          tVertices[2] = cpvrotate(tVertices[2], tRot);
00093          tVertices[3] = cpvrotate(tVertices[3], tRot);
00094          /* Create the shape */
00095          m_ptShape =
00096             cpSpaceAddShape(m_cDyn2DEngine.GetPhysicsSpace(),
00097                             cpPolyShapeNew(m_ptBody,
00098                                            4,
00099                                            tVertices,
00100                                            cpv(cPosition.GetX(), cPosition.GetY())));
00101          
00102          m_ptShape->e = 0.0; // No elasticity
00103          m_ptShape->u = 0.1; // Little contact friction to help sliding away
00104          /* This shape is normal (not grippable, not gripper) */
00105          m_ptShape->collision_type = CDynamics2DEngine::SHAPE_NORMAL;
00106       }
00107       /* Associate this model to the body data for ray queries */
00108       m_ptBody->data = this;
00109       /* Calculate bounding box */
00110       GetBoundingBox().MinCorner.SetZ(GetEmbodiedEntity().GetPosition().GetZ());
00111       GetBoundingBox().MaxCorner.SetZ(GetEmbodiedEntity().GetPosition().GetZ() + m_cBoxEntity.GetSize().GetZ());
00112       CalculateBoundingBox();
00113    }
00114    
00115    /****************************************/
00116    /****************************************/
00117 
00118    CDynamics2DBoxModel::~CDynamics2DBoxModel() {
00119       if(m_bMovable) {
00120          delete m_pcGrippable;
00121          cpSpaceRemoveConstraint(m_cDyn2DEngine.GetPhysicsSpace(), m_ptLinearFriction);
00122          cpSpaceRemoveConstraint(m_cDyn2DEngine.GetPhysicsSpace(), m_ptAngularFriction);
00123          cpConstraintFree(m_ptLinearFriction);
00124          cpConstraintFree(m_ptAngularFriction);
00125          cpSpaceRemoveShape(m_cDyn2DEngine.GetPhysicsSpace(), m_ptShape);
00126          cpSpaceRemoveBody(m_cDyn2DEngine.GetPhysicsSpace(), m_ptBody);
00127          cpShapeFree(m_ptShape);
00128          cpBodyFree(m_ptBody);
00129       }
00130       else {
00131          cpSpaceRemoveShape(m_cDyn2DEngine.GetPhysicsSpace(), m_ptShape);
00132          cpShapeFree(m_ptShape);
00133          cpBodyFree(m_ptBody);
00134          cpSpaceReindexStatic(m_cDyn2DEngine.GetPhysicsSpace());
00135       }
00136    }
00137 
00138    /****************************************/
00139    /****************************************/
00140 
00141    bool CDynamics2DBoxModel::MoveTo(const CVector3& c_position,
00142                                     const CQuaternion& c_orientation,
00143                                     bool b_check_only) {
00144       SInt32 nCollision;
00145       /* Check whether the box is movable or not */
00146       if(m_bMovable) {
00147          /* The box is movable */
00148          /* Save body position and orientation */
00149          cpVect tOldPos = m_ptBody->p;
00150          cpFloat fOldA = m_ptBody->a;
00151          /* Move the body to the desired position */
00152          m_ptBody->p = cpv(c_position.GetX(), c_position.GetY());
00153          CRadians cXAngle, cYAngle, cZAngle;
00154          c_orientation.ToEulerAngles(cZAngle, cYAngle, cXAngle);
00155          cpBodySetAngle(m_ptBody, cZAngle.GetValue());
00156          /* Create a shape sensor to test the movement */
00157          /* First construct the vertices */
00158          CVector3 cHalfSize = m_cBoxEntity.GetSize() * 0.5f;
00159          cpVect tVertices[] = {
00160             cpv(-cHalfSize.GetX(), -cHalfSize.GetY()),
00161             cpv(-cHalfSize.GetX(),  cHalfSize.GetY()),
00162             cpv( cHalfSize.GetX(),  cHalfSize.GetY()),
00163             cpv( cHalfSize.GetX(), -cHalfSize.GetY())
00164          };
00165          /* Then create the shape itself */
00166          cpShape* ptTestShape = cpPolyShapeNew(m_ptBody,
00167                                                4,
00168                                                tVertices,
00169                                                cpvzero);
00170          /* Check if there is a collision */
00171          nCollision = cpSpaceShapeQuery(m_cDyn2DEngine.GetPhysicsSpace(), ptTestShape, NULL, NULL);
00172          /* Dispose of the sensor shape */
00173          cpShapeFree(ptTestShape);
00174          if(b_check_only || nCollision) {
00175             /* Restore old body state if there was a collision or
00176                it was only a check for movement */
00177             m_ptBody->p = tOldPos;
00178             cpBodySetAngle(m_ptBody, fOldA);
00179          }
00180          else {
00181             /* Object moved, release all gripped entities */
00182             m_pcGrippable->ReleaseAll();
00183             /* Update the active space hash if the movement is actual */
00184             cpSpaceReindexShape(m_cDyn2DEngine.GetPhysicsSpace(), m_ptShape);
00185             /* Update bounding box */
00186             CalculateBoundingBox();
00187          }
00188       }
00189       else {
00190          /* The box is not movable, so you can't move it :-) */
00191          nCollision = 1;
00192       }
00193       /* The movement is allowed if there is no collision */
00194       return !nCollision;
00195    }
00196 
00197    /****************************************/
00198    /****************************************/
00199 
00200    void CDynamics2DBoxModel::Reset() {
00201       if(m_bMovable) {
00202          /* Reset body position */
00203          const CVector3& cPosition = GetEmbodiedEntity().GetInitPosition();
00204          m_ptBody->p = cpv(cPosition.GetX(), cPosition.GetY());
00205          /* Reset body orientation */
00206          CRadians cXAngle, cYAngle, cZAngle;
00207          GetEmbodiedEntity().GetInitOrientation().ToEulerAngles(cZAngle, cYAngle, cXAngle);
00208          cpBodySetAngle(m_ptBody, cZAngle.GetValue());
00209          /* Zero speed and applied forces */
00210          m_ptBody->v = cpvzero;
00211          m_ptBody->w = 0.0f;
00212          cpBodyResetForces(m_ptBody);
00213          /* Update bounding box */
00214          cpShapeCacheBB(m_ptShape);
00215          CalculateBoundingBox();
00216          /* Release all attached entities */
00217          m_pcGrippable->ReleaseAll();
00218       }
00219    }
00220 
00221    /****************************************/
00222    /****************************************/
00223 
00224    void CDynamics2DBoxModel::CalculateBoundingBox() {
00225       GetBoundingBox().MinCorner.SetX(m_ptShape->bb.l);
00226       GetBoundingBox().MinCorner.SetY(m_ptShape->bb.b);
00227       GetBoundingBox().MaxCorner.SetX(m_ptShape->bb.r);
00228       GetBoundingBox().MaxCorner.SetY(m_ptShape->bb.t);
00229    }
00230 
00231    /****************************************/
00232    /****************************************/
00233 
00234    void CDynamics2DBoxModel::UpdateEntityStatus() {
00235       if(m_bMovable) {
00236          /* Update bounding box */
00237          CalculateBoundingBox();
00238          /* Update entity position and orientation */
00239          m_cDyn2DEngine.PositionPhysicsToSpace(m_cSpacePosition, GetEmbodiedEntity().GetPosition(), m_ptBody);
00240          GetEmbodiedEntity().SetPosition(m_cSpacePosition);
00241          m_cDyn2DEngine.OrientationPhysicsToSpace(m_cSpaceOrientation, m_ptBody);
00242          GetEmbodiedEntity().SetOrientation(m_cSpaceOrientation);
00243       }
00244       /* Update components */
00245       m_cBoxEntity.UpdateComponents();
00246    }
00247 
00248    /****************************************/
00249    /****************************************/
00250 
00251    bool CDynamics2DBoxModel::IsCollidingWithSomething() const {
00252       return cpSpaceShapeQuery(m_cDyn2DEngine.GetPhysicsSpace(), m_ptShape, NULL, NULL) > 0;
00253    }
00254 
00255    /****************************************/
00256    /****************************************/
00257 
00258    REGISTER_STANDARD_DYNAMICS2D_OPERATIONS_ON_ENTITY(CBoxEntity, CDynamics2DBoxModel);
00259 
00260    /****************************************/
00261    /****************************************/
00262 
00263 }