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