ARGoS  3
A parallel, multi-engine simulator for swarm robotics
plugins/simulator/entities/wheeled_entity.cpp
Go to the documentation of this file.
00001 
00007 #include "wheeled_entity.h"
00008 #include <argos3/core/simulator/space/space.h>
00009 
00010 namespace argos {
00011 
00012    /****************************************/
00013    /****************************************/
00014 
00015    CWheeledEntity::CWheeledEntity(CComposableEntity* pc_parent,
00016                                   size_t un_num_wheels) :
00017       CEntity(pc_parent),
00018       m_unNumWheels(un_num_wheels) {
00019       m_pcWheelPositions = new CVector3[m_unNumWheels];
00020       ::memset(m_pcWheelPositions, 0, m_unNumWheels * sizeof(CVector3));
00021       m_pfWheelRadia = new Real[m_unNumWheels];
00022       ::memset(m_pfWheelRadia, 0, m_unNumWheels * sizeof(Real));
00023       m_pfWheelVelocities = new Real[m_unNumWheels];
00024       ::memset(m_pfWheelVelocities, 0, m_unNumWheels * sizeof(Real));
00025    }
00026 
00027    /****************************************/
00028    /****************************************/
00029 
00030    CWheeledEntity::CWheeledEntity(CComposableEntity* pc_parent,
00031                                   const std::string& str_id,
00032                                   size_t un_num_wheels) :
00033       CEntity(pc_parent, str_id),
00034       m_unNumWheels(un_num_wheels) {
00035       m_pcWheelPositions = new CVector3[m_unNumWheels];
00036       ::memset(m_pcWheelPositions, 0, m_unNumWheels * sizeof(CVector3));
00037       m_pfWheelRadia = new Real[m_unNumWheels];
00038       ::memset(m_pfWheelRadia, 0, m_unNumWheels * sizeof(Real));
00039       m_pfWheelVelocities = new Real[m_unNumWheels];
00040       ::memset(m_pfWheelVelocities, 0, m_unNumWheels * sizeof(Real));
00041    }
00042    
00043    /****************************************/
00044    /****************************************/
00045 
00046    CWheeledEntity::~CWheeledEntity() {
00047       delete[] m_pcWheelPositions;
00048       delete[] m_pfWheelRadia;
00049       delete[] m_pfWheelVelocities;
00050    }
00051    
00052    /****************************************/
00053    /****************************************/
00054    
00055    void CWheeledEntity::Reset() {
00056       ::memset(m_pfWheelVelocities, 0, m_unNumWheels * sizeof(Real));
00057    }
00058    
00059    /****************************************/
00060    /****************************************/
00061    
00062    void CWheeledEntity::SetWheel(UInt32 un_index,
00063                                  const CVector3& c_position,
00064                                  Real f_radius) {
00065       if(un_index < m_unNumWheels) {
00066          m_pcWheelPositions[un_index] = c_position;
00067          m_pfWheelRadia[un_index] = f_radius;
00068       }
00069       else {
00070          THROW_ARGOSEXCEPTION("CWheeledEntity::SetWheel() : index " << un_index << " out of bounds (allowed [0:" << m_unNumWheels << "])");
00071       }
00072    }
00073 
00074    /****************************************/
00075    /****************************************/
00076 
00077    const CVector3& CWheeledEntity::GetWheelPosition(size_t un_index) const {
00078       if(un_index < m_unNumWheels) {
00079          return m_pcWheelPositions[un_index];
00080       }
00081       else {
00082          THROW_ARGOSEXCEPTION("CWheeledEntity::GetWheelPosition() : index " << un_index << " out of bounds (allowed [0:" << m_unNumWheels << "])");
00083       }
00084    }
00085    
00086    /****************************************/
00087    /****************************************/
00088    
00089    Real CWheeledEntity::GetWheelRadius(size_t un_index) const {
00090       if(un_index < m_unNumWheels) {
00091          return m_pfWheelRadia[un_index];
00092       }
00093       else {
00094          THROW_ARGOSEXCEPTION("CWheeledEntity::GetWheelRadius() : index " << un_index << " out of bounds (allowed [0:" << m_unNumWheels << "])");
00095       }
00096    }
00097    
00098    /****************************************/
00099    /****************************************/
00100    
00101    Real CWheeledEntity::GetWheelVelocity(size_t un_index) const {
00102       if(un_index < m_unNumWheels) {
00103          return m_pfWheelRadia[un_index];
00104       }
00105       else {
00106          THROW_ARGOSEXCEPTION("CWheeledEntity::GetWheelVelocity() : index " << un_index << " out of bounds (allowed [0:" << m_unNumWheels << "])");
00107       }
00108    }
00109    
00110    /****************************************/
00111    /****************************************/
00112    
00113    void CWheeledEntity::SetVelocities(Real* pf_velocities) {
00114       ::memcpy(m_pfWheelVelocities, pf_velocities, m_unNumWheels * sizeof(Real));
00115    }
00116 
00117    /****************************************/
00118    /****************************************/
00119 
00120    REGISTER_STANDARD_SPACE_OPERATIONS_ON_ENTITY(CWheeledEntity);
00121 
00122    /****************************************/
00123    /****************************************/
00124 
00125 }