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