예제 #1
0
 bool CDynamics3DEntity::MoveTo(const CVector3& c_position,
                                const CQuaternion& c_orientation,
                                bool b_check_only) {
    /* Move the body to the new position */
    dBodySetPosition(m_tBody, c_position.GetX(), c_position.GetY(), c_position.GetZ());
    /* Rotate the body to the new orientation */
    dQuaternion tQuat = { c_orientation.GetW(),
                          c_orientation.GetX(),
                          c_orientation.GetY(),
                          c_orientation.GetZ() };
    dBodySetQuaternion(m_tBody, tQuat);
    /* Check for collisions */
    bool bCollisions = m_cEngine.IsEntityColliding(m_tEntitySpace);
    if(bCollisions || b_check_only) {
       /*
        * Undo the changes if there is a collision or
        * if the move was just a check
        */
       const CVector3& cPosition = GetEmbodiedEntity().GetPosition();
       dBodySetPosition(m_tBody, cPosition.GetX(), cPosition.GetY(), cPosition.GetZ());
       const CQuaternion& cOrientation = GetEmbodiedEntity().GetOrientation();
       dQuaternion tQuat2 = { cOrientation.GetW(),
                              cOrientation.GetX(),
                              cOrientation.GetY(),
                              cOrientation.GetZ() };
       dBodySetQuaternion(m_tBody, tQuat2);
       return !bCollisions;
    }
    else {
       /* Set the new position and orientation */
       GetEmbodiedEntity().SetPosition(c_position);
       GetEmbodiedEntity().SetOrientation(c_orientation);
       return true;
    }
 }
예제 #2
0
 void CPointMass3DSpiriModel::UpdateCameraAnchor(SAnchor& s_anchor) {
    s_anchor.Orientation = GetEmbodiedEntity().GetOriginAnchor().Orientation;
    s_anchor.Orientation *= s_anchor.OffsetOrientation;
    s_anchor.Position = s_anchor.OffsetPosition;
    s_anchor.Position.Rotate(s_anchor.Orientation);
    s_anchor.Position += GetEmbodiedEntity().GetOriginAnchor().Position;
 }
 void CKinematics2DEntity::Reset() {
   /* Reset body position */
   m_cPosition = GetEmbodiedEntity().GetInitPosition().ProjectOntoXY();
   
   /* Reset body orientation */
   CRadians cXAngle, cYAngle;
   GetEmbodiedEntity().GetInitOrientation().ToEulerAngles(m_cOrientation, cYAngle, cXAngle);
 }
 void CKinematics2DEntity::UpdateEntityStatus() {
   /* Update entity position */
   m_cEngine.PositionPhysicsToSpace(m_cSpacePosition, GetEmbodiedEntity().GetPosition(), m_cPosition);
   GetEmbodiedEntity().SetPosition(m_cSpacePosition);
   
   /* Update entity orientation */
   m_cEngine.OrientationPhysicsToSpace(m_cSpaceOrientation, m_cOrientation);
   GetEmbodiedEntity().SetOrientation(m_cSpaceOrientation);
 }
   void CDynamics2DBoxEntity::UpdateEntityStatus() {
      if(m_ptBody != NULL) {
         m_cEngine.PositionPhysicsToSpace(m_cSpacePosition, GetEmbodiedEntity().GetPosition(), m_ptBody);
         GetEmbodiedEntity().SetPosition(m_cSpacePosition);
         m_cEngine.OrientationPhysicsToSpace(m_cSpaceOrientation, m_ptBody);
         GetEmbodiedEntity().SetOrientation(m_cSpaceOrientation);
      }

      /* Update components */
      m_cBoxEntity.UpdateComponents();
   }
예제 #6
0
 void CPhysXMultiBodyObjectModel::Reset() {
    /* Move the bodies to the initial position and orientation */
    MoveTo(GetEmbodiedEntity().GetOriginAnchor().Position,
           GetEmbodiedEntity().GetOriginAnchor().Orientation);
    /* Clear all forces */
    for(size_t i = 0; i < m_vecBodies.size(); ++i) {
       m_vecBodies[i].Body.clearForce();
       m_vecBodies[i].Body.clearTorque();
    }
    /* Update ARGoS entity status */
    UpdateEntityStatus();
 }
 void CDynamics2DSingleBodyObjectModel::SetBody(cpBody* pt_body,
                                                Real f_height) {
    /* Set the body and its data field for ray queries */
    m_ptBody = pt_body;
    m_ptBody->data = this;
    /* Register the origin anchor update method */
    RegisterAnchorMethod(GetEmbodiedEntity().GetOriginAnchor(),
                         &CDynamics2DSingleBodyObjectModel::UpdateOriginAnchor);
    /* Calculate the bounding box */
    GetBoundingBox().MinCorner.SetZ(GetEmbodiedEntity().GetOriginAnchor().Position.GetZ());
    GetBoundingBox().MaxCorner.SetZ(GetEmbodiedEntity().GetOriginAnchor().Position.GetZ() + f_height);
    CalculateBoundingBox();
 }
 void CDynamics2DMultiBodyObjectModel::Reset() {
    /* Reset body position */
    MoveTo(GetEmbodiedEntity().GetOriginAnchor().Position,
           GetEmbodiedEntity().GetOriginAnchor().Orientation);
    /* For each body: */
    for(size_t i = 0; i < m_vecBodies.size(); ++i) {
       /* Zero the speeds */
       m_vecBodies[i].Body->v = cpvzero;
       m_vecBodies[i].Body->w = 0.0f;
       /* Zero forces and torques */
       cpBodyResetForces(m_vecBodies[i].Body);
    }
 }
예제 #9
0
 void CDynamics3DEntity::UpdateEntityStatus() {
    /* Update entity position and orientation */
    const dReal* ptPosition = dBodyGetPosition(m_tBody);
    GetEmbodiedEntity().SetPosition(
       CVector3(ptPosition[0],
                ptPosition[1],
                ptPosition[2]));
    const dReal* ptOrientation = dBodyGetQuaternion(m_tBody);
    GetEmbodiedEntity().SetOrientation(
       CQuaternion(ptOrientation[0],
                   ptOrientation[1],
                   ptOrientation[2],
                   ptOrientation[3]));
 }
예제 #10
0
void CDynamics2DCylinderEntity::Reset() {
    if(m_ptBody != NULL) {
        /* Reset body position */
        const CVector3& cPosition = GetEmbodiedEntity().GetInitPosition();
        m_ptBody->p = cpv(cPosition.GetX(), cPosition.GetY());
        /* Reset body orientation */
        CRadians cXAngle, cYAngle, cZAngle;
        GetEmbodiedEntity().GetInitOrientation().ToEulerAngles(cZAngle, cYAngle, cXAngle);
        cpBodySetAngle(m_ptBody, cZAngle.GetValue());
        /* Zero speed and applied forces */
        m_ptBody->v = cpvzero;
        m_ptBody->w = 0.0f;
        cpBodyResetForces(m_ptBody);
    }
}
  void CKinematics2DEntity::UpdateFromEntityStatus() {
    if( m_bEnabled ) {
      m_cPreviousPosition = m_cPosition;
      m_cPreviousOrientation = m_cOrientation;

      GetEmbodiedEntity().ClearCollisionDetected();
    }
  }
  CKinematics2DEntity::CKinematics2DEntity(CKinematics2DEngine& c_engine, CEmbodiedEntity& c_entity) :
    CPhysicsEngineEntity(c_entity),
    m_cEngine(c_engine),
    m_cPosition(),
    m_cOrientation(),
    m_cSpacePosition(),
    m_cSpaceOrientation(),
    m_bEnabled(false),
    m_tCollisionType(KINEMATICS2D_COLLISION_NONE) {

    /* Set the initial body position */
    m_cPosition = GetEmbodiedEntity().GetPosition().ProjectOntoXY();
    
    /* set the initial body orientation */
    CRadians cXAngle, cYAngle;
    GetEmbodiedEntity().GetOrientation().ToEulerAngles(m_cOrientation, cYAngle, cXAngle);
  }
 void CDynamics2DSingleBodyObjectModel::Reset() {
    /* Nothing to do for a static body */
    if(cpBodyIsStatic(m_ptBody)) return;
    /* Reset body position */
    const CVector3& cPosition = GetEmbodiedEntity().GetOriginAnchor().Position;
    m_ptBody->p = cpv(cPosition.GetX(), cPosition.GetY());
    /* Reset body orientation */
    CRadians cXAngle, cYAngle, cZAngle;
    GetEmbodiedEntity().GetOriginAnchor().Orientation.ToEulerAngles(cZAngle, cYAngle, cXAngle);
    cpBodySetAngle(m_ptBody, cZAngle.GetValue());
    /* Zero speed and applied forces */
    m_ptBody->v = cpvzero;
    m_ptBody->w = 0.0f;
    cpBodyResetForces(m_ptBody);
    /* Update bounding box */
    cpSpaceReindexShapesForBody(GetDynamics2DEngine().GetPhysicsSpace(), m_ptBody);
    CalculateBoundingBox();
 }
예제 #14
0
 CDynamics3DBox::CDynamics3DBox(CDynamics3DEngine& c_engine,
                                CBoxEntity& c_box) :
    CDynamics3DEntity(c_engine, c_box.GetEmbodiedEntity()),
    m_cBoxEntity(c_box),
    m_sGeomData(GEOM_NORMAL) {
    /* Check whether the box is movable or not */
    if(c_box.GetEmbodiedEntity().IsMovable()) {
       /* Movable box */
       /* Set the body to its initial position and orientation */
       const CQuaternion& cOrient = GetEmbodiedEntity().GetOrientation();
       dQuaternion tQuat = { cOrient.GetW(), cOrient.GetX(), cOrient.GetY(), cOrient.GetZ() };
       dBodySetQuaternion(m_tBody, tQuat);
       const CVector3& cPos = GetEmbodiedEntity().GetPosition();
       dBodySetPosition(m_tBody, cPos.GetX(), cPos.GetY(), cPos.GetZ());
       /* Create the geometry and the mass */
       const CVector3& cBoxSize = c_box.GetSize();
       m_tGeom = dCreateBox(m_tEntitySpace, cBoxSize.GetX(), cBoxSize.GetY(), cBoxSize.GetZ());
       /* Set Geom gripping properties. */
       m_sGeomData.Type = GEOM_GRIPPABLE;
       dGeomSetData(m_tGeom, &m_sGeomData);
       /* Create its mass */
       dMassSetBoxTotal(&m_tMass, c_box.GetMass(), cBoxSize.GetX(), cBoxSize.GetY(), cBoxSize.GetZ());
       /* Associate the body to the geom */
       dGeomSetBody(m_tGeom, m_tBody);
       /* Set the parent body total mass */
       dBodySetMass(m_tBody, &m_tMass);
    }
    else {
       /* Unmovable box, get rid of the body and add only the geometry */
       dBodyDestroy(m_tBody);
       /* Create the geometry */
       const CVector3& cBoxSize = c_box.GetSize();
       m_tGeom = dCreateBox(m_tEntitySpace, cBoxSize.GetX(), cBoxSize.GetY(), cBoxSize.GetZ());
       dGeomSetData(m_tGeom, &m_sGeomData);
       /* Set the geom to its position and orientation */
       const CQuaternion& cOrient = GetEmbodiedEntity().GetOrientation();
       dQuaternion tQuat = { cOrient.GetW(), cOrient.GetX(), cOrient.GetY(), cOrient.GetZ() };
       dGeomSetQuaternion(m_tGeom, tQuat);
       const CVector3& cPos = GetEmbodiedEntity().GetPosition();
       dGeomSetPosition(m_tGeom, cPos.GetX(), cPos.GetY(), cPos.GetZ());
       /* Associate the geom to null body (this makes it static) */
       dGeomSetBody(m_tGeom, 0);
    }
 }
예제 #15
0
 void CCylinderEntity::UpdateComponents() {
    /* Set LED position and update led equipped entity */
    CVector3 cLEDPosition;
    for(UInt32 i = 0; i < m_pcLEDEquippedEntity->GetAllLeds().size(); ++i) {
       cLEDPosition = m_vecBaseLEDPositions[i];
       cLEDPosition.Rotate(m_pcEmbodiedEntity->GetOrientation());
       cLEDPosition += GetEmbodiedEntity().GetPosition();
       m_pcLEDEquippedEntity->SetLedPosition(i, cLEDPosition);
    }
    m_pcEmbodiedEntity->UpdateBoundingBox();
 }
예제 #16
0
bool CDynamics2DCylinderEntity::CheckIntersectionWithRay(Real& f_t_on_ray,
        const CRay& c_ray) const {
    cpSegmentQueryInfo tInfo;
    if(cpShapeSegmentQuery(m_ptShape,
                           cpv(c_ray.GetStart().GetX(), c_ray.GetStart().GetY()),
                           cpv(c_ray.GetEnd().GetX()  , c_ray.GetEnd().GetY()  ),
                           &tInfo)) {
        CVector3 cIntersectionPoint;
        c_ray.GetPoint(cIntersectionPoint, tInfo.t);
        if((cIntersectionPoint.GetZ() >= GetEmbodiedEntity().GetPosition().GetZ() - m_fHalfHeight) &&
                (cIntersectionPoint.GetZ() <= GetEmbodiedEntity().GetPosition().GetZ() + m_fHalfHeight) ) {
            f_t_on_ray = tInfo.t;
            return true;
        }
        else {
            return false;
        }
    }
    else {
        return false;
    }
}
예제 #17
0
 CPointMass3DSpiriModel::CPointMass3DSpiriModel(CPointMass3DEngine& c_engine,
                                                CSpiriEntity& c_spiri) :
    CPointMass3DQuadRotorModel(c_engine,
                               c_spiri.GetEmbodiedEntity(),
                               c_spiri.GetQuadRotorEntity(),
                               BODY_HEIGHT,
                               BODY_RADIUS,
                               BODY_MASS,
                               BODY_INERTIA,
                               POS_K_P,
                               POS_K_D,
                               YAW_K_P,
                               YAW_K_D,
                               VEL_K_P,
                               VEL_K_D,
                               ROT_K_P,
                               ROT_K_D) {
    /* Register anchor update methods */
    RegisterAnchorMethod(GetEmbodiedEntity().GetAnchor("rab"),
                         &CPointMass3DSpiriModel::UpdateRABAnchor);
    RegisterAnchorMethod(GetEmbodiedEntity().GetAnchor("camera"),
                         &CPointMass3DSpiriModel::UpdateCameraAnchor);
 }
예제 #18
0
 void CDynamics3DEntity::Reset() {
    /* Clear force and torque on the body */
    dBodySetForce(m_tBody, 0.0f, 0.0f, 0.0f);
    dBodySetTorque(m_tBody, 0.0f, 0.0f, 0.0f);
    /* Clear speeds */
    dBodySetLinearVel(m_tBody, 0.0f, 0.0f, 0.0f);
    dBodySetAngularVel(m_tBody, 0.0f, 0.0f, 0.0f);
    /* Reset position */
    const CVector3& cPosition = GetEmbodiedEntity().GetInitPosition();
    dBodySetPosition(m_tBody,
                     cPosition.GetX(),
                     cPosition.GetY(),
                     cPosition.GetZ());
    /* Reset orientation */
    const CQuaternion& cQuaternion = GetEmbodiedEntity().GetInitOrientation();
    dQuaternion tQuat = {
       cQuaternion.GetW(),
       cQuaternion.GetX(),
       cQuaternion.GetY(),
       cQuaternion.GetZ()
    };
    dBodySetQuaternion(m_tBody, tQuat);
 }
예제 #19
0
 void CEPuckEntity::SetLedPosition() {
    /* Set LED positions */
    const CVector3& cEntityPosition = GetEmbodiedEntity().GetPosition();
    CVector3 cLEDPosition;
    CRadians cLEDAngle;
    SET_RING_LED_POSITION(0);
    SET_RING_LED_POSITION(1);
    SET_RING_LED_POSITION(2);
    SET_RING_LED_POSITION(3);
    SET_RING_LED_POSITION(4);
    SET_RING_LED_POSITION(5);
    SET_RING_LED_POSITION(6);
    SET_RING_LED_POSITION(7);
 }
예제 #20
0
 CDynamics2DEPuckModel::CDynamics2DEPuckModel(CDynamics2DEngine& c_engine,
                                              CEPuckEntity& c_entity) :
    CDynamics2DSingleBodyObjectModel(c_engine, c_entity),
    m_cEPuckEntity(c_entity),
    m_cWheeledEntity(m_cEPuckEntity.GetWheeledEntity()),
    m_cDiffSteering(c_engine,
                    EPUCK_MAX_FORCE,
                    EPUCK_MAX_TORQUE,
                    EPUCK_INTERWHEEL_DISTANCE),
    m_fCurrentWheelVelocity(m_cWheeledEntity.GetWheelVelocities()) {
    /* Create the body with initial position and orientation */
    cpBody* ptBody =
       cpSpaceAddBody(GetDynamics2DEngine().GetPhysicsSpace(),
                      cpBodyNew(EPUCK_MASS,
                                cpMomentForCircle(EPUCK_MASS,
                                                  0.0f,
                                                  EPUCK_RADIUS + EPUCK_RADIUS,
                                                  cpvzero)));
    const CVector3& cPosition = GetEmbodiedEntity().GetOriginAnchor().Position;
    ptBody->p = cpv(cPosition.GetX(), cPosition.GetY());
    CRadians cXAngle, cYAngle, cZAngle;
    GetEmbodiedEntity().GetOriginAnchor().Orientation.ToEulerAngles(cZAngle, cYAngle, cXAngle);
    cpBodySetAngle(ptBody, cZAngle.GetValue());
    /* Create the body shape */
    cpShape* ptShape =
       cpSpaceAddShape(GetDynamics2DEngine().GetPhysicsSpace(),
                       cpCircleShapeNew(ptBody,
                                        EPUCK_RADIUS,
                                        cpvzero));
    ptShape->e = 0.0; // No elasticity
    ptShape->u = 0.7; // Lots of friction
    /* Constrain the actual base body to follow the diff steering control */
    m_cDiffSteering.AttachTo(ptBody);
    /* Set the body so that the default methods work as expected */
    SetBody(ptBody, EPUCK_HEIGHT);
 }
예제 #21
0
 void CPhysicsModel::UpdateEntityStatus() {
    CalculateAnchors();
    CalculateBoundingBox();
    /*
     * Update entity components
     */
    /* Get a reference to the root entity */
    /* NOTE: here the cast is static because we know that an embodied entity MUST have a parent
     * which, by definition, is a composable entity */
    CComposableEntity& cRoot = static_cast<CComposableEntity&>(m_cEmbodiedEntity.GetRootEntity());
    /* Update its components */
    cRoot.UpdateComponents();
    /*
     * Check whether a transfer is necessary
     */
    if(!m_cEngine.IsPointContained(GetEmbodiedEntity().GetOriginAnchor().Position))
       m_cEngine.ScheduleEntityForTransfer(m_cEmbodiedEntity);
 }
/**
 * Calculate the AABB in the global coordinate frame
 */
void CBulletSphereModel::CalculateBoundingBox()
{
	GetBoundingBox().MinCorner.Set(
					GetEmbodiedEntity().GetOriginAnchor().Position.GetX() - (entity->GetRadius()),
					GetEmbodiedEntity().GetOriginAnchor().Position.GetY() - (entity->GetRadius()),
					GetEmbodiedEntity().GetOriginAnchor().Position.GetZ()
			);
	GetBoundingBox().MaxCorner.Set(
					GetEmbodiedEntity().GetOriginAnchor().Position.GetX() + (entity->GetRadius()),
					GetEmbodiedEntity().GetOriginAnchor().Position.GetY() + (entity->GetRadius()),
					GetEmbodiedEntity().GetOriginAnchor().Position.GetZ() + entity->GetRadius()
			);
}
예제 #23
0
 void CFootBotEntity::SetLedPosition() {
    /* Set LED positions */
    const CVector3& cEntityPosition = GetEmbodiedEntity().GetPosition();
    CVector3 cLEDPosition;
    CRadians cLEDAnglePhase = FOOTBOT_HALF_LED_ANGLE_SLICE + m_cTurretRotation;
    CRadians cLEDAngle;
    SET_RING_LED_POSITION(0);
    SET_RING_LED_POSITION(1);
    SET_RING_LED_POSITION(2);
    SET_RING_LED_POSITION(3);
    SET_RING_LED_POSITION(4);
    SET_RING_LED_POSITION(5);
    SET_RING_LED_POSITION(6);
    SET_RING_LED_POSITION(7);
    SET_RING_LED_POSITION(8);
    SET_RING_LED_POSITION(9);
    SET_RING_LED_POSITION(10);
    SET_RING_LED_POSITION(11);
    /* Set beacon position */
    cLEDPosition.Set(0.0f, 0.0f, FOOTBOT_BEACON_ELEVATION);
    cLEDPosition.Rotate(m_pcEmbodiedEntity->GetOrientation());
    cLEDPosition += cEntityPosition;
    m_pcLEDEquippedEntity->SetLedPosition(12, cLEDPosition);
 }
예제 #24
0
CDynamics2DCylinderEntity::CDynamics2DCylinderEntity(CDynamics2DEngine& c_engine,
        CCylinderEntity& c_entity) :
    CDynamics2DEntity(c_engine, c_entity.GetEmbodiedEntity()),
    m_cCylinderEntity(c_entity),
    m_fMass(c_entity.GetMass()),
    m_ptShape(NULL),
    m_ptBody(NULL) {
    /* Get the radius of the entity */
    Real fRadius = c_entity.GetRadius();
    m_fHalfHeight = c_entity.GetHeight() * 0.5f;
    /* Create a circle object in the physics space */
    const CVector3& cPosition = GetEmbodiedEntity().GetPosition();
    if(c_entity.IsMovable()) {
        /* The cylinder is movable */
        /* Create the body */
        m_ptBody = cpSpaceAddBody(m_cEngine.GetPhysicsSpace(),
                                  cpBodyNew(m_fMass,
                                            cpMomentForCircle(m_fMass,
                                                    0,
                                                    fRadius + fRadius,
                                                    cpvzero)));
        m_ptBody->p = cpv(cPosition.GetX(), cPosition.GetY());
        CRadians cXAngle, cYAngle, cZAngle;
        GetEmbodiedEntity().GetOrientation().ToEulerAngles(cZAngle, cYAngle, cXAngle);
        cpBodySetAngle(m_ptBody, cZAngle.GetValue());
        /* Create the geometry */
        m_ptShape = cpSpaceAddShape(m_cEngine.GetPhysicsSpace(),
                                    cpCircleShapeNew(m_ptBody, fRadius, cpvzero));
        /* This object is grippable */
        m_ptShape->collision_type = CDynamics2DEngine::SHAPE_GRIPPABLE;
        m_ptShape->data = reinterpret_cast<void*>(&c_entity);
        /* No elasticity */
        m_ptShape->e = 0.0;
        /* Lots surface contact friction to help pushing */
        m_ptShape->u = 0.7;
        /* Friction with ground */
        m_ptLinearFriction =
            cpSpaceAddConstraint(m_cEngine.GetPhysicsSpace(),
                                 cpPivotJointNew2(m_cEngine.GetGroundBody(),
                                         m_ptBody,
                                         cpvzero,
                                         cpvzero));
        m_ptLinearFriction->biasCoef = 0.0f; // disable joint correction
        m_ptLinearFriction->maxForce = 1.0f; // emulate linear friction
        m_ptAngularFriction =
            cpSpaceAddConstraint(m_cEngine.GetPhysicsSpace(),
                                 cpGearJointNew(m_cEngine.GetGroundBody(),
                                                m_ptBody,
                                                0.0f,
                                                1.0f));
        m_ptAngularFriction->biasCoef = 0.0f; // disable joint correction
        m_ptAngularFriction->maxForce = 5.0f; // emulate angular friction
    }
    else {
        /* The cylinder is not movable */
        /* Create the geometry */
        m_ptShape = cpSpaceAddStaticShape(m_cEngine.GetPhysicsSpace(),
                                          cpCircleShapeNew(m_cEngine.GetGroundBody(),
                                                  fRadius,
                                                  cpv(cPosition.GetX(), cPosition.GetY())));
        /* This object is normal */
        m_ptShape->collision_type = CDynamics2DEngine::SHAPE_NORMAL;
        m_ptShape->data = reinterpret_cast<void*>(&c_entity);
        /* No elasticity */
        m_ptShape->e = 0.0;
        /* Little contact friction to help sliding away */
        m_ptShape->u = 0.1;
    }
}
 CDynamics2DBoxEntity::CDynamics2DBoxEntity(CDynamics2DEngine& c_engine,
                                            CBoxEntity& c_entity) :
    CDynamics2DEntity(c_engine, c_entity.GetEmbodiedEntity()),
    m_cBoxEntity(c_entity),
    m_fMass(c_entity.GetMass()),
    m_ptShape(NULL),
    m_ptBody(NULL) {
    /* Get the size of the entity */
    CVector3 cHalfSize = c_entity.GetSize() * 0.5f;
    m_fHalfHeight = cHalfSize.GetZ();
    /* Create a polygonal object in the physics space */
    /* Start defining the vertices
       NOTE: points must be defined in a clockwise winding
    */
    cpVect tVertices[] = {
       cpv(-cHalfSize.GetX(), -cHalfSize.GetY()),
       cpv(-cHalfSize.GetX(),  cHalfSize.GetY()),
       cpv( cHalfSize.GetX(),  cHalfSize.GetY()),
       cpv( cHalfSize.GetX(), -cHalfSize.GetY())
    };
    const CVector3& cPosition = GetEmbodiedEntity().GetPosition();
    CRadians cXAngle, cYAngle, cZAngle;
    GetEmbodiedEntity().GetOrientation().ToEulerAngles(cZAngle, cYAngle, cXAngle);
    if(c_entity.GetEmbodiedEntity().IsMovable()) {
       /* The box is movable */
       /* Create the body */
       m_ptBody =
          cpSpaceAddBody(m_cEngine.GetPhysicsSpace(),
                         cpBodyNew(m_fMass,
                                   cpMomentForPoly(m_fMass,
                                                   4,
                                                   tVertices,
                                                   cpvzero)));
       m_ptBody->p = cpv(cPosition.GetX(), cPosition.GetY());
       cpBodySetAngle(m_ptBody, cZAngle.GetValue());
       /* Create the geometry */
       m_ptShape =
          cpSpaceAddShape(m_cEngine.GetPhysicsSpace(),
                          cpPolyShapeNew(m_ptBody,
                                         4,
                                         tVertices,
                                         cpvzero));
       /* This object is grippable */
       m_ptShape->collision_type = CDynamics2DEngine::SHAPE_GRIPPABLE;
       m_ptShape->data = reinterpret_cast<void*>(&c_entity);
       /* No elasticity */
       m_ptShape->e = 0.0;
       /* Lots contact friction to help pushing */
       m_ptShape->u = 0.7;
       /* Friction with ground */
       m_ptLinearFriction =
          cpSpaceAddConstraint(m_cEngine.GetPhysicsSpace(),
                               cpPivotJointNew2(m_cEngine.GetGroundBody(),
                                                m_ptBody,
                                                cpvzero,
                                                cpvzero));
       m_ptLinearFriction->maxBias = 0.0f; // disable joint correction
       m_ptLinearFriction->maxForce = 1.49f; // emulate linear friction (this is just slightly smaller than FOOTBOT_MAX_FORCE)
       m_ptAngularFriction =
          cpSpaceAddConstraint(m_cEngine.GetPhysicsSpace(),
                               cpGearJointNew(m_cEngine.GetGroundBody(),
                                              m_ptBody,
                                              0.0f,
                                              1.0f));
       m_ptAngularFriction->maxBias = 0.0f; // disable joint correction
       m_ptAngularFriction->maxForce = 1.49f; // emulate angular friction (this is just slightly smaller than FOOTBOT_MAX_TORQUE)
    }
    else {
       /* The box is not movable */
       /* Manually rotate the vertices */
       cpVect tRot = cpvforangle(cZAngle.GetValue());
       tVertices[0] = cpvrotate(tVertices[0], tRot);
       tVertices[1] = cpvrotate(tVertices[1], tRot);
       tVertices[2] = cpvrotate(tVertices[2], tRot);
       tVertices[3] = cpvrotate(tVertices[3], tRot);
       /* Create the geometry */
       m_ptShape =
          cpSpaceAddStaticShape(m_cEngine.GetPhysicsSpace(),
                                cpPolyShapeNew(m_cEngine.GetGroundBody(),
                                               4,
                                               tVertices,
                                               cpv(cPosition.GetX(), cPosition.GetY())));
       /* This object is normal */
       m_ptShape->collision_type = CDynamics2DEngine::SHAPE_NORMAL;
       m_ptShape->data = reinterpret_cast<void*>(&c_entity);
       /* No elasticity */
       m_ptShape->e = 0.0;
       /* Little contact friction to help sliding away */
       m_ptShape->u = 0.1;
    }
 }