pinocchio  3.7.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
 
Loading...
Searching...
No Matches
geometry.hpp
1//
2// Copyright (c) 2021-2022 INRIA
3//
4
5#ifndef __pinocchio_multibody_pool_geometry_hpp__
6#define __pinocchio_multibody_pool_geometry_hpp__
7
8#include "pinocchio/multibody/geometry.hpp"
9#include "pinocchio/multibody/pool/model.hpp"
10
11namespace pinocchio
12{
13 template<typename _Scalar, int _Options, template<typename, int> class JointCollectionTpl>
14 class GeometryPoolTpl : public ModelPoolTpl<_Scalar, _Options, JointCollectionTpl>
15 {
16 public:
18
20 typedef _Scalar Scalar;
21 enum
22 {
23 Options = _Options
24 };
25
26 typedef typename Base::Model Model;
27 typedef typename Base::Data Data;
28 typedef typename Base::ModelVector ModelVector;
29 typedef typename Base::DataVector DataVector;
32
33 typedef std::vector<GeometryModel, Eigen::aligned_allocator<GeometryModel>> GeometryModelVector;
34 typedef std::vector<GeometryData, Eigen::aligned_allocator<GeometryData>> GeometryDataVector;
35
43 const Model & model,
45 const size_t pool_size = (size_t)omp_get_max_threads())
46 : Base(model, pool_size)
47 {
50 for (size_t k = 0; k < pool_size; ++k)
51 {
52 m_geometry_models.push_back(geometry_model.clone());
54
55 typedef typename GeometryData::SE3 SE3;
56 for (SE3 & oMg_i : m_geometry_datas.back().oMg)
57 {
58 oMg_i.setIdentity();
59 }
60 }
61 }
62
68 : Base(other)
69 {
70 const size_t pool_size = other.size();
73 for (size_t k = 0; k < pool_size; ++k)
74 {
75 m_geometry_models.push_back(other.m_geometry_models[k].clone());
77 }
78 }
79
81 const GeometryModel & getGeometryModel(const size_t index) const
82 {
83 PINOCCHIO_CHECK_INPUT_ARGUMENT(
84 index < m_geometry_models.size(),
85 "Index greater than the size of the geometry_models vector.");
86 return m_geometry_models[index];
87 }
88
90 GeometryModel & getGeometryModel(const size_t index)
91 {
92 PINOCCHIO_CHECK_INPUT_ARGUMENT(
93 index < m_geometry_models.size(),
94 "Index greater than the size of the geometry_models vector.");
95 return m_geometry_models[index];
96 }
97
99 const GeometryData & getGeometryData(const size_t index) const
100 {
101 PINOCCHIO_CHECK_INPUT_ARGUMENT(
102 index < m_geometry_datas.size(),
103 "Index greater than the size of the geometry_datas vector.");
104 return m_geometry_datas[index];
105 }
106
108 GeometryData & getGeometryData(const size_t index)
109 {
110 PINOCCHIO_CHECK_INPUT_ARGUMENT(
111 index < m_geometry_datas.size(),
112 "Index greater than the size of the geometry_datas vector.");
113 return m_geometry_datas[index];
114 }
115
117 const GeometryDataVector & getGeometryDatas() const
118 {
119 return m_geometry_datas;
120 }
121
123 GeometryDataVector & getGeometryDatas()
124 {
125 return m_geometry_datas;
126 }
127
129 const GeometryModelVector & getGeometryModels() const
130 {
131 return m_geometry_models;
132 }
133
135 GeometryModelVector & getGeometryModels()
136 {
137 return m_geometry_models;
138 }
139
140 using Base::size;
141 using Base::update;
142
145 void sync(const GeometryModel & geometry_model, const std::vector<GeomIndex> & geometry_indexes)
146 {
147 for (GeomIndex i : geometry_indexes)
148 PINOCCHIO_CHECK_INPUT_ARGUMENT(
149 i < geometry_model.ngeoms,
150 "One of the given geometry index is greater than geometry_model.ngeoms.");
151
153 {
154 for (GeomIndex i : geometry_indexes)
155 geometry_model_pool.geometryObjects[i] = geometry_model.geometryObjects[i].clone();
156 }
157 }
158
165 {
167 {
169 geometry_data.activeCollisionPairs = geometry_data_to_copy.activeCollisionPairs;
170 geometry_data.distanceRequests = geometry_data_to_copy.distanceRequests;
171 geometry_data.collisionRequests = geometry_data_to_copy.collisionRequests;
172 geometry_data.collisionPairIndex = geometry_data_to_copy.collisionPairIndex;
173 }
174 }
175
177 virtual ~GeometryPoolTpl() {};
178
179 protected:
181 GeometryModelVector m_geometry_models;
182
184 GeometryDataVector m_geometry_datas;
185
187 virtual void doResize(const size_t new_size)
188 {
189 const size_t current_size = (size_t)size();
190 m_geometry_models.resize((size_t)new_size);
191 m_geometry_datas.resize((size_t)new_size);
193 {
194 for (size_t k = current_size; k < new_size; ++k)
195 {
198 }
199 }
200 }
201 };
202
203} // namespace pinocchio
204
205#endif // ifndef __pinocchio_multibody_pool_geometry_hpp__
const GeometryModel & getGeometryModel(const size_t index) const
Returns the geometry_model at given index.
Definition geometry.hpp:81
size_t size() const
Returns the size of the pool.
Definition model.hpp:68
GeometryModelVector m_geometry_models
Vector of Geometry Model associated to the pool.
Definition geometry.hpp:181
virtual void update(const GeometryData &geometry_data_to_copy)
Update the geometry datas with the new value.
Definition geometry.hpp:164
GeometryData & getGeometryData(const size_t index)
Returns the geometry_data at given index.
Definition geometry.hpp:108
GeometryDataVector & getGeometryDatas()
Returns the vector of Geometry Data.
Definition geometry.hpp:123
const GeometryModelVector & getGeometryModels() const
Returns the vector of Geometry Model.
Definition geometry.hpp:129
GeometryModel & getGeometryModel(const size_t index)
Returns the geometry_model at given index.
Definition geometry.hpp:90
void sync(const GeometryModel &geometry_model, const std::vector< GeomIndex > &geometry_indexes)
Synchronize the internal geometry models with the input geometry for all given geometry indexes by cl...
Definition geometry.hpp:145
GeometryPoolTpl(const Model &model, const GeometryModel &geometry_model, const size_t pool_size=(size_t) omp_get_max_threads())
Default constructor from a model and a pool size.
Definition geometry.hpp:42
virtual void doResize(const size_t new_size)
&#160;
Definition geometry.hpp:187
GeometryPoolTpl(const GeometryPoolTpl &other)
Copy constructor from an other GeometryPoolTpl.
Definition geometry.hpp:67
GeometryDataVector m_geometry_datas
Vector of Geometry Data associated to the pool.
Definition geometry.hpp:184
const GeometryDataVector & getGeometryDatas() const
Returns the vector of Geometry Data.
Definition geometry.hpp:117
virtual ~GeometryPoolTpl()
&#160;
Definition geometry.hpp:177
const GeometryData & getGeometryData(const size_t index) const
Returns the geometry_data at given index.
Definition geometry.hpp:99
GeometryModelVector & getGeometryModels()
Returns the vector of Geometry Model.
Definition geometry.hpp:135
size_t size() const
Returns the size of the pool.
Definition model.hpp:68
void update(const Data &data)
Update all the datas with the input data value.
Definition model.hpp:62
Main pinocchio namespace.
Definition treeview.dox:11