Warning: include(php/utility.php): Failed to open stream: No such file or directory in /home/argos/argos3/doc/api/embedded/a00831_source.php on line 2

Warning: include(): Failed opening 'php/utility.php' for inclusion (include_path='.:/usr/lib64/php') in /home/argos/argos3/doc/api/embedded/a00831_source.php on line 2
The ARGoS Website

dynamics3d_model.cpp
Go to the documentation of this file.
1 
7 #include "dynamics3d_model.h"
8 
9 #include <argos3/core/simulator/physics_engine/physics_model.h>
10 #include <argos3/core/simulator/entity/composable_entity.h>
11 #include <argos3/core/utility/math/vector3.h>
12 #include <argos3/core/utility/math/quaternion.h>
13 #include <argos3/plugins/simulator/physics_engines/dynamics3d/dynamics3d_engine.h>
14 
15 namespace argos {
16 
17  /****************************************/
18  /****************************************/
19 
20  CDynamics3DModel::CAbstractBody::SData::SData(const btTransform& c_start_transform,
21  const btTransform& c_center_of_mass_offset,
22  const btVector3& c_inertia,
23  btScalar f_mass,
24  btScalar f_friction) :
25  StartTransform(c_start_transform),
26  InverseStartTransform(c_start_transform.inverse()),
27  CenterOfMassOffset(c_center_of_mass_offset),
28  InverseCenterOfMassOffset(c_center_of_mass_offset.inverse()),
29  Inertia(c_inertia),
30  Mass(f_mass),
31  Friction(f_friction) {}
32 
33  /****************************************/
34  /****************************************/
35 
37  SAnchor& s_anchor,
38  std::shared_ptr<btCollisionShape>& ptr_shape,
39  const SData& s_data) :
40  m_cModel(c_model),
41  m_sAnchor(s_anchor),
42  m_ptrShape(ptr_shape),
43  m_sData(s_data) {}
44 
45  /****************************************/
46  /****************************************/
47 
49  return m_cModel;
50  }
51 
52  /****************************************/
53  /****************************************/
54 
56  return m_sAnchor;
57  }
58 
59  /****************************************/
60  /****************************************/
61 
63  return *(m_ptrShape.get());
64  }
65 
66  /****************************************/
67  /****************************************/
68 
70  return m_sData;
71  }
72 
73  /****************************************/
74  /****************************************/
75 
77  /* Offset the world transform to respect ARGoS conventions */
78  btTransform cTransform =
79  GetTransform() * m_sData.CenterOfMassOffset;
80  /* Retrieve the position and orientation */
81  const btVector3& cPosition = cTransform.getOrigin();
82  const btQuaternion& cOrientation = cTransform.getRotation();
83  /* Swap coordinate system and set anchor position */
84  m_sAnchor.Position.Set(cPosition.getX(), -cPosition.getZ(), cPosition.getY());
85  /* Swap coordinate system and set anchor orientation */
86  m_sAnchor.Orientation.Set(cOrientation.getW(),
87  cOrientation.getX(),
88  -cOrientation.getZ(),
89  cOrientation.getY());
90  }
91 
92  /****************************************/
93  /****************************************/
94 
96  CComposableEntity& c_entity) :
97  CPhysicsModel(c_engine, c_entity.GetComponent<CEmbodiedEntity>("body")),
98  m_cEngine(c_engine),
99  m_cComposableEntity(c_entity) {}
100 
101  /****************************************/
102  /****************************************/
103 
105  while(! m_vecBodies.empty()) {
106  delete m_vecBodies.back();
107  m_vecBodies.pop_back();
108  }
109  }
110 
111  /****************************************/
112  /****************************************/
113 
115  /* Rerun collision detection */
116  m_cEngine.GetWorld().performDiscreteCollisionDetection();
117  /* Get the collision dispatcher */
118  const btCollisionDispatcher& cDispatcher =
119  m_cEngine.GetDispatcher();
120  /* For each manifold from the collision dispatcher */
121  for(SInt32 i = 0; i < cDispatcher.getNumManifolds(); i++) {
122  const btPersistentManifold* pcContactManifold =
123  cDispatcher.getManifoldByIndexInternal(i);
124  const CDynamics3DModel* pcModelA =
125  static_cast<const CDynamics3DModel*>(pcContactManifold->getBody0()->getUserPointer());
126  const CDynamics3DModel* pcModelB =
127  static_cast<const CDynamics3DModel*>(pcContactManifold->getBody1()->getUserPointer());
128  /* Ignore self-collisions */
129  if(pcModelA == pcModelB) {
130  continue;
131  }
132  /* Ignore collisions that don't belong to a model (e.g. a collision with the ground) */
133  if((pcModelA == nullptr) || (pcModelB == nullptr)) {
134  continue;
135  }
136  /* Only check collisions that involve this model */
137  if((pcModelA == this) || (pcModelB == this)) {
138  /* One of the two bodies involved in the contact manifold belongs to this model,
139  check for contact points with negative distance to indicate a collision */
140  for(SInt32 j = 0; j < pcContactManifold->getNumContacts(); j++) {
141  const btManifoldPoint& cManifoldPoint = pcContactManifold->getContactPoint(j);
142  if (cManifoldPoint.getDistance() < 0.0f) {
143  return true;
144  }
145  }
146  }
147  }
148  return false;
149  }
150 
151  /****************************************/
152  /****************************************/
153 
155  /* Update the anchor associated with each body before running the base class's UpdateEntityStatus
156  which updates the bounding box and origin anchor */
157  for(CAbstractBody* pc_body : m_vecBodies) {
158  pc_body->UpdateAnchor();
159  }
161  }
162 
163  /****************************************/
164  /****************************************/
165 
166 }
167 
signed int SInt32
32-bit signed integer.
Definition: datatypes.h:93
std::shared_ptr< btCollisionShape > m_ptrShape
CAbstractBody(CDynamics3DModel &c_model, SAnchor &s_anchor, std::shared_ptr< btCollisionShape > &ptr_shape, const SData &s_data)
This entity is a link to a body in the physics engine.
std::vector< CAbstractBody * > m_vecBodies
btMultiBodyDynamicsWorld & GetWorld()
virtual bool IsCollidingWithSomething() const
Returns true if this model is colliding with another model.
virtual void UpdateEntityStatus()
Updates the status of the associated entity.
Basic class for an entity that contains other entities.
An anchor related to the body of an entity.
Definition: physics_model.h:38
btCollisionDispatcher & GetDispatcher()
SData(const btTransform &c_start_transform, const btTransform &c_center_of_mass_offset, const btVector3 &c_inertia, btScalar f_mass, btScalar f_friction)
CDynamics3DModel(CDynamics3DEngine &c_engine, CComposableEntity &c_entity)
virtual void UpdateEntityStatus()
Updates the status of the associated entity.
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12