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>
21 const btTransform& c_center_of_mass_offset,
22 const btVector3& c_inertia,
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()),
31 Friction(f_friction) {}
38 std::shared_ptr<btCollisionShape>& ptr_shape,
39 const SData& s_data) :
63 return *(m_ptrShape.get());
78 btTransform cTransform =
79 GetTransform() * m_sData.CenterOfMassOffset;
81 const btVector3& cPosition = cTransform.getOrigin();
82 const btQuaternion& cOrientation = cTransform.getRotation();
84 m_sAnchor.Position.Set(cPosition.getX(), -cPosition.getZ(), cPosition.getY());
86 m_sAnchor.Orientation.Set(cOrientation.getW(),
99 m_cComposableEntity(c_entity) {}
116 m_cEngine.
GetWorld().performDiscreteCollisionDetection();
118 const btCollisionDispatcher& cDispatcher =
121 for(
SInt32 i = 0; i < cDispatcher.getNumManifolds(); i++) {
122 const btPersistentManifold* pcContactManifold =
123 cDispatcher.getManifoldByIndexInternal(i);
125 static_cast<const CDynamics3DModel*
>(pcContactManifold->getBody0()->getUserPointer());
127 static_cast<const CDynamics3DModel*
>(pcContactManifold->getBody1()->getUserPointer());
129 if(pcModelA == pcModelB) {
133 if((pcModelA ==
nullptr) || (pcModelB ==
nullptr)) {
137 if((pcModelA ==
this) || (pcModelB ==
this)) {
140 for(
SInt32 j = 0; j < pcContactManifold->getNumContacts(); j++) {
141 const btManifoldPoint& cManifoldPoint = pcContactManifold->getContactPoint(j);
142 if (cManifoldPoint.getDistance() < 0.0f) {
158 pc_body->UpdateAnchor();
signed int SInt32
32-bit signed integer.
virtual ~CDynamics3DModel()
std::shared_ptr< btCollisionShape > m_ptrShape
CDynamics3DModel & GetModel()
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.
btCollisionDispatcher & GetDispatcher()
btCollisionShape & GetShape()
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.
CDynamics3DModel & m_cModel