pointmass3d_footbot_model.cpp
Go to the documentation of this file.
1 
8 #include <argos3/core/utility/math/cylinder.h>
9 
10 namespace argos {
11 
12  static const Real FOOTBOT_RADIUS = 0.085036758f;
13  static const Real FOOTBOT_INTERWHEEL_DISTANCE = 0.14f;
14  static const Real FOOTBOT_HEIGHT = 0.146899733f;
15 
19  };
20 
21  /****************************************/
22  /****************************************/
23 
25  CFootBotEntity& c_footbot) :
26  CPointMass3DModel(c_engine, c_footbot.GetEmbodiedEntity()),
27  m_cWheeledEntity(c_footbot.GetWheeledEntity()),
28  m_fCurrentWheelVelocity(m_cWheeledEntity.GetWheelVelocities()) {
29  /* Register the origin anchor update method */
30  RegisterAnchorMethod(GetEmbodiedEntity().GetOriginAnchor(),
32  /* Get initial rotation */
33  CRadians cTmp1, cTmp2;
35  }
36 
37  /****************************************/
38  /****************************************/
39 
42  CRadians cTmp1, cTmp2;
44  m_fAngularVelocity = 0.0;
45  }
46 
47  /****************************************/
48  /****************************************/
49 
50 
52  m_cVelocity.Set((m_fCurrentWheelVelocity[FOOTBOT_RIGHT_WHEEL] + m_fCurrentWheelVelocity[FOOTBOT_LEFT_WHEEL])*0.5, 0.0, 0.0);
53  m_cVelocity.RotateZ(m_cYaw);
54  m_fAngularVelocity = (m_fCurrentWheelVelocity[FOOTBOT_RIGHT_WHEEL] - m_fCurrentWheelVelocity[FOOTBOT_LEFT_WHEEL]) / FOOTBOT_INTERWHEEL_DISTANCE;
55  }
56 
57  /****************************************/
58  /****************************************/
59 
62  m_cYaw += CRadians(m_fAngularVelocity * m_cPM3DEngine.GetPhysicsClockTick());
63  }
64 
65  /****************************************/
66  /****************************************/
67 
70  GetEmbodiedEntity().GetOriginAnchor().Position.GetX() - FOOTBOT_RADIUS,
71  GetEmbodiedEntity().GetOriginAnchor().Position.GetY() - FOOTBOT_RADIUS,
74  GetEmbodiedEntity().GetOriginAnchor().Position.GetX() + FOOTBOT_RADIUS,
75  GetEmbodiedEntity().GetOriginAnchor().Position.GetY() + FOOTBOT_RADIUS,
76  GetEmbodiedEntity().GetOriginAnchor().Position.GetZ() + FOOTBOT_HEIGHT);
77  }
78 
79  /****************************************/
80  /****************************************/
81 
83  const CRay3& c_ray) const {
84  CCylinder m_cShape(FOOTBOT_RADIUS,
85  FOOTBOT_HEIGHT,
87  CVector3::Z);
88  bool bIntersects = m_cShape.Intersects(f_t_on_ray, c_ray);
89  return bIntersects;
90  }
91 
92  /****************************************/
93  /****************************************/
94 
96  s_anchor.Position = m_cPosition;
97  s_anchor.Orientation = CQuaternion(m_cYaw, CVector3::Z);
98  }
99 
100  /****************************************/
101  /****************************************/
102 
104 
105  /****************************************/
106  /****************************************/
107 
108 }
void RegisterAnchorMethod(const SAnchor &s_anchor, void(MODEL::*pt_method)(SAnchor &))
Registers an anchor method.
REGISTER_STANDARD_POINTMASS3D_OPERATIONS_ON_ENTITY(CEyeBotEntity, CPointMass3DEyeBotModel)
const SBoundingBox & GetBoundingBox() const
Returns an axis-aligned box that contains the physics model.
An anchor related to the body of an entity.
Definition: physics_model.h:38
void Set(const Real f_x, const Real f_y, const Real f_z)
Sets the vector contents from Cartesian coordinates.
Definition: vector3.h:143
bool Intersects(Real &f_t_on_ray, const CRay3 &c_ray)
Definition: cylinder.cpp:9
virtual void UpdateFromEntityStatus()
Updates the state of this model from the status of the associated entity.
void ToEulerAngles(CRadians &c_z_angle, CRadians &c_y_angle, CRadians &c_x_angle) const
Definition: quaternion.h:172
const SAnchor & GetOriginAnchor() const
Returns a const reference to the origin anchor associated to this entity.
CPointMass3DEngine & m_cPM3DEngine
Reference to the physics engine.
Real GetY() const
Returns the y coordinate of this vector.
Definition: vector3.h:109
virtual void UpdateOriginAnchor(SAnchor &s_anchor)
Updates the origin anchor associated to the embodied entity.
virtual bool CheckIntersectionWithRay(Real &f_t_on_ray, const CRay3 &c_ray) const
CVector3 & RotateZ(const CRadians &c_angle)
Rotates this vector wrt the z axis.
Definition: vector3.h:265
CVector3 Position
The position of the anchor wrt the global coordinate system.
Definition: physics_model.h:51
CQuaternion Orientation
The orientation of the anchor wrt the global coordinate system.
Definition: physics_model.h:53
Real GetZ() const
Returns the z coordinate of this vector.
Definition: vector3.h:125
Real GetPhysicsClockTick() const
Returns the length of the physics engine tick.
CEmbodiedEntity & GetEmbodiedEntity()
Returns the embodied entity associated to this physics model.
It defines the basic type CRadians, used to store an angle value in radians.
Definition: angles.h:42
CPointMass3DFootBotModel(CPointMass3DEngine &c_engine, CFootBotEntity &c_footbot)
virtual void CalculateBoundingBox()
Calculates the axis-aligned box that contains the entire physics model.
CVector3 m_cPosition
The position of the model in this engine.
static const CVector3 Z
The z axis.
Definition: vector3.h:40
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
float Real
Collects all ARGoS code.
Definition: datatypes.h:39
CVector3 m_cVelocity
The linear velocity of this model in the engine.