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