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