Choreonoid  1.5
Deque2D.h
Go to the documentation of this file.
1 
6 #ifndef CNOID_UTIL_DEQUE_2D_H
7 #define CNOID_UTIL_DEQUE_2D_H
8 
9 #include <Eigen/StdVector>
10 #include <memory>
11 #include <iterator>
12 
13 namespace cnoid {
14 
15 template <typename ElementType, typename Allocator = std::allocator<ElementType> >
16 class Deque2D
17 {
19 
20 public:
21  typedef ElementType Element;
22 
23  class const_iterator : public std::iterator<std::random_access_iterator_tag, ElementType> {
24 
25  friend class Deque2D<ElementType, Allocator>;
26 
27  protected:
28  ElementType* current;
29  ElementType* term;
30  ElementType* buf;
31 
32  const_iterator(const Deque2DType& owner, ElementType* pos) {
33  current = pos;
34  buf = owner.buf;
35  term = buf + owner.capacity_;
36  }
37 
38  public:
40 
42  current = org.current;
43  term = org.term;
44  buf = org.buf;
45  }
46 
47  const Element& operator*() const {
48  return *current;
49  }
51  ++current;
52  if(current == term){
53  current = buf;
54  }
55  return *this;
56  }
58  if(current == buf){
59  current = term - 1;
60  } else {
61  --current;
62  }
63  }
65  current += n;
66  if(current >= term){
67  current = buf + (current - term);
68  }
69  return *this;
70  }
72  current -= n;
73  if(current < buf){
74  current = term - (buf - current);
75  }
76  return *this;
77  }
79  const_iterator iter(*this);
80  iter += n;
81  return iter;
82  }
84  const_iterator iter(*this);
85  iter -= n;
86  return iter;
87  }
88  bool operator==(const const_iterator& rhs) const {
89  return (current == rhs.current);
90  }
91  bool operator!=(const const_iterator& rhs) const {
92  return (current != rhs.current);
93  }
94  };
95 
96  class iterator : public const_iterator {
97 
98  friend class Deque2D<ElementType, Allocator>;
99 
100  iterator(Deque2DType& owner, ElementType* pos) : const_iterator(owner, pos) { }
101 
102  public:
103  iterator() { }
104  iterator(const iterator& org) : const_iterator(org) { }
105 
106  Element& operator*() {
107  return *const_iterator::current;
108  }
109  iterator& operator+=(size_t n){
113  }
114  return *this;
115  }
116  iterator& operator-=(size_t n){
120  }
121  return *this;
122  }
123  iterator operator+(size_t n){
124  iterator iter(*this);
125  iter += n;
126  return iter;
127  }
128  iterator operator-(size_t n){
129  iterator iter(*this);
130  iter -= n;
131  return iter;
132  }
133  };
134 
135  iterator begin() {
136  return iterator(*this, buf + offset);
137  }
138 
140  return const_iterator(*this, buf + offset);
141  }
142 
143  iterator end() {
144  return end_;
145  }
146 
148  return end_;
149  }
150 
151  class Row
152  {
153  ElementType* top;
154  int size_;
155 
156  public:
157  Row() {
158  size_ = 0;
159  }
160 
161  Row(const Deque2D<ElementType, Allocator>& owner, int rowIndex) {
162  size_ = owner.colSize_;
163  top = owner.buf;
164  if(owner.capacity_ > 0){
165  top += (owner.offset + rowIndex * owner.colSize_) % owner.capacity_;
166  }
167  }
168 
169  bool empty() const {
170  return (size_ == 0);
171  }
172 
173  int size() const {
174  return size_;
175  }
176 
177  Element& operator[](int index){
178  return top[index];
179  }
180 
181  const Element& operator[](int index) const {
182  return top[index];
183  }
184 
185  Element& at(int index) {
186  return top[index];
187  }
188 
189  Element* begin() {
190  return top;
191  }
192 
193  Element* end() {
194  return top + size_;
195  }
196  };
197 
198  class Column
199  {
200  public:
201  Column() {
202  colSize = 0;
203  rowSize = 0;
204  }
205 
207  top = owner.buf + column;
208  offset = owner.offset;
209  colSize = owner.colSize_;
210  capacity = owner.capacity_;
211  rowSize = owner.rowSize_;
212  end_ = iterator(*this, top + ((owner.capacity_ > 0) ? ((offset + owner.size_) % owner.capacity_) : 0));
213  }
214 
215  bool empty() const {
216  return (rowSize == 0);
217  }
218 
219  int size() const {
220  return rowSize;
221  }
222 
223  Element& operator[](int rowIndex){
224  return top[(offset + rowIndex * colSize) % capacity];
225  }
226 
227  const Element& operator[](int rowIndex) const {
228  return top[(offset + rowIndex * colSize) % capacity];
229  }
230 
231  Element& at(int index) {
232  return top[index * colSize];
233  }
234 
235  class iterator : public std::iterator<std::bidirectional_iterator_tag, ElementType, int> {
236 
237  ElementType* current;
238  ElementType* term;
239  ElementType* top;
240  int colSize;
241 
242  public:
243 
244  iterator() { }
245 
246  iterator(Column& column, Element* pos){
247  current = pos;
248  top = column.top;
249  term = top + column.capacity;
250  colSize = column.colSize;
251  }
252  Element& operator*() {
253  return *current;
254  }
255  void operator++() {
256  current += colSize;
257  if(current == term){
258  current = top;
259  }
260  }
261  void operator--() {
262  if(current == top){
263  current = term - colSize;
264  } else {
265  current -= colSize;
266  }
267  }
268  bool operator==(iterator rhs) const {
269  return (current == rhs.current);
270  }
271  bool operator!=(iterator rhs) const {
272  return (current != rhs.current);
273  }
274  };
275 
277  return iterator(*this, top + offset);
278  }
280  return end_;
281  }
282 
283  private:
284  ElementType* top;
285  int offset;
286  int colSize;
287  int capacity;
288  int rowSize;
289  typename Column::iterator end_;
290  };
291 
293  buf = 0;
294  offset = 0;
295  rowSize_ = 0;
296  colSize_ = 0;
297  capacity_ = 0;
298  size_ = 0;
299  }
300 
301  Deque2D(int rowSize, int colSize) {
302 
303  buf = 0;
304  offset = 0;
305  rowSize_ = 0;
306  colSize_ = 0;
307  capacity_ = 0;
308  size_ = 0;
309 
310  resizeMain(rowSize, colSize, false);
311  }
312 
314  : allocator(org.allocator) {
315 
316  size_ = org.size_;
317  rowSize_ = org.rowSize_;
318  colSize_ = org.colSize_;
319  capacity_ = size_ + colSize_;
320  buf = 0;
321 
322  if(capacity_){
323  buf = allocator.allocate(capacity_);
324  offset = 0;
325  ElementType* p = buf;
326  ElementType* pend = buf + size_;
327  ElementType* q = org.buf + org.offset;
328  ElementType* qterm = org.buf + org.capacity_;
329  while(p != pend){
330  allocator.construct(p++, *q++);
331  if(q == qterm){
332  q = org.buf;
333  }
334  }
335  }
336  end_ = iterator(*this, buf + ((capacity_ > 0) ? ((offset + size_) % capacity_) : 0));
337  }
338 
339  Deque2DType& operator=(const Deque2DType& rhs) {
340  if(this != &rhs){
341  resizeMain(rhs.rowSize_, rhs.colSize_, false);
342  iterator p = begin();
343  const_iterator q = rhs.cbegin();
344  const_iterator qend = rhs.cend();
345  while(q != qend){
346  *p = *q;
347  ++p;
348  ++q;
349  }
350  }
351  return *this;
352  }
353 
354  virtual ~Deque2D() {
355  if(buf){
356  ElementType* p = buf + offset;
357  const ElementType* pend = buf + (offset + size_) % capacity_;
358 
359  if(p <= pend){
360  while(p != pend){
361  allocator.destroy(p++);
362  }
363  } else {
364  for(ElementType* q = buf; q != pend; ++q){
365  allocator.destroy(q);
366  }
367  const ElementType* pterm = buf + capacity_;
368  for(ElementType* q = p; q != pterm; ++q){
369  allocator.destroy(q);
370  }
371  }
372  allocator.deallocate(buf, capacity_);
373  }
374  }
375 
376  bool empty() const {
377  return !rowSize_ || !colSize_;
378  }
379 
380 private:
381  void reallocMemory(int newColSize, int newSize, int newCapacity, bool doCopy) {
382 
383  ElementType* newBuf;
384  if(newCapacity > 0){
385  newBuf = allocator.allocate(newCapacity);
386  } else {
387  newBuf = 0;
388  }
389  ElementType* p = newBuf;
390  ElementType* pend = newBuf + newSize;
391 
392  if(capacity_ > 0){
393  ElementType* qend = buf + (offset + size_) % capacity_;
394 
395  // copy the existing elements
396  if(newCapacity > 0 && newColSize == colSize_ && doCopy){
397  ElementType* q = buf + offset;
398  if(q <= qend){
399  while(q != qend && p != pend){
400  allocator.construct(p++, *q++);
401  }
402  } else {
403  for(ElementType* r = buf; r != qend && p != pend; ++r){
404  allocator.construct(p++, *r);
405  }
406  ElementType* qterm = buf + capacity_;
407  for(ElementType* r = q; r != qterm && p != pend; ++r){
408  allocator.construct(p++, *r);
409  }
410  }
411  }
412  // destory the old elements
413  ElementType* q = buf + offset;
414  if(q <= qend){
415  while(q != qend){
416  allocator.destroy(q++);
417  }
418  } else {
419  for(ElementType* r = buf; r != qend; ++r){
420  allocator.destroy(r);
421  }
422  ElementType* qterm = buf + capacity_;
423  for(ElementType* r = q; r != qterm; ++r){
424  allocator.destroy(r);
425  }
426  }
427  }
428 
429  // construct new elements
430  while(p != pend){
431  allocator.construct(p++, ElementType());
432  }
433 
434  if(buf){
435  allocator.deallocate(buf, capacity_);
436  }
437  buf = newBuf;
438  capacity_ = newCapacity;
439  offset = 0;
440  }
441 
442  void resizeMain(int newRowSize, int newColSize, bool doCopy) {
443 
444  const int newSize = newRowSize * newColSize;
445 
446  if(newSize == 0){
447  reallocMemory(newColSize, newSize, 0, false);
448 
449  } else {
450  // The area for the 'end' iterator should be reserved
451  const int minCapacity = newSize + newColSize;
452 
453  if(capacity_ > 0 && minCapacity <= capacity_){
454  if(newColSize != colSize_ && (capacity_ % newColSize > 0)){
455  reallocMemory(newColSize, newSize, capacity_ - (capacity_ % newColSize), doCopy);
456 
457  } else if(newSize > size_){
458  ElementType* p = buf + (offset + size_) % capacity_;
459  const ElementType* pend = buf + (offset + newSize) % capacity_;
460  if(p <= pend){
461  while(p != pend){
462  allocator.construct(p++, ElementType());
463  }
464  } else {
465  for(ElementType* r = buf; r != pend; ++r){
466  allocator.construct(r, ElementType());
467  }
468  const ElementType* pterm = buf + capacity_;
469  for(ElementType* r = p; r != pterm; ++r){
470  allocator.construct(r, ElementType());
471  }
472  }
473  } else if(newSize < size_){
474  ElementType* p = buf + (offset + newSize) % capacity_;
475  ElementType* pend = buf + (offset + size_) % capacity_;
476  if(p <= pend){
477  while(p != pend){
478  allocator.destroy(p++);
479  }
480  } else {
481  for(ElementType* r = buf; r != pend; ++r){
482  allocator.destroy(r);
483  }
484  const ElementType* pterm = buf + capacity_;
485  for(ElementType* r = p; r != pterm; ++r){
486  allocator.destroy(r);
487  }
488  }
489  }
490  } else {
491  if(!buf){
492  capacity_ = minCapacity;
493  if(capacity_ > 0){
494  buf = allocator.allocate(minCapacity);
495  ElementType* p = buf;
496  ElementType* pend = buf + newSize;
497  // construct new elements
498  while(p != pend){
499  allocator.construct(p++, ElementType());
500  }
501  }
502  } else {
503  int newCapacity;
504  const int expandedSize = size_ * 3 / 2;
505  if(expandedSize > newSize){
506  newCapacity = expandedSize - (expandedSize % newColSize) + newColSize;
507  } else {
508  newCapacity = minCapacity;
509  }
510  reallocMemory(newColSize, newSize, newCapacity, doCopy);
511  }
512  }
513  }
514 
515  rowSize_ = newRowSize;
516  colSize_ = newColSize;
517  size_ = newSize;
518  end_ = iterator(*this, buf + ((capacity_ > 0) ? ((offset + size_) % capacity_) : 0));
519  }
520 
521 public:
522  void resize(int newRowSize, int newColSize) {
523  resizeMain(newRowSize, newColSize, true);
524  }
525 
526  void resizeColumn(int newColSize){
527  resize(rowSize_, newColSize);
528  }
529 
530  int rowSize() const {
531  return rowSize_;
532  }
533 
537  void resizeRow(int newRowSize){
538  resize(newRowSize, colSize_);
539  }
540 
541  int colSize() const {
542  return colSize_;
543  }
544 
545  void clear() {
546  resize(0, 0);
547  }
548 
549  const Element& operator()(int rowIndex, int colIndex) const {
550  return buf[(offset + (rowIndex * colSize_)) % capacity_ + colIndex];
551  }
552 
553  Element& operator()(int rowIndex, int colIndex) {
554  return buf[(offset + (rowIndex * colSize_)) % capacity_ + colIndex];
555  }
556 
557  const Element& at(int rowIndex, int colIndex) const {
558  return buf[(offset + (rowIndex * colSize_)) % capacity_ + colIndex];
559  }
560 
561  Element& at(int rowIndex, int colIndex) {
562  return buf[(offset + (rowIndex * colSize_)) % capacity_ + colIndex];
563  }
564 
565  Row operator[](int rowIndex) {
566  return Row(*this, rowIndex);
567  }
568 
569  const Row operator[](int rowIndex) const {
570  return Row(*this, rowIndex);
571  }
572 
573  Row row(int rowIndex) {
574  return Row(*this, rowIndex);
575  }
576 
577  const Row row(int rowIndex) const {
578  return Row(*this, rowIndex);
579  }
580 
581  Row last() {
582  return Row(*this, rowSize_ - 1);
583  }
584 
585  const Row last() const {
586  return Row(*this, rowSize_ - 1);
587  }
588 
589  Column column(int colIndex) {
590  return Column(*this, colIndex);
591  }
592 
593  const Column column(int colIndex) const {
594  return Column(*this, colIndex);
595  }
596 
597  Row append() {
598  resize(rowSize_ + 1, colSize_);
599  return Row(*this, rowSize_ - 1);
600  }
601 
602  void pop_back() {
603  resize(rowSize_ - 1, colSize_);
604  }
605 
606  void pop_front(int numRows) {
607  if(numRows <= 0){
608  return;
609  }
610  if(numRows > rowSize_){
611  numRows = rowSize_;
612  }
613  const size_t popSize = numRows * colSize_;
614  ElementType* p = buf + offset;
615  const ElementType* pend = buf + (offset + popSize) % capacity_;
616 
617  if(p <= pend){
618  while(p != pend){
619  allocator.destroy(p++);
620  }
621  } else {
622  for(ElementType* r = buf; r != pend; ++r){
623  allocator.destroy(r);
624  }
625  const ElementType* pterm = buf + capacity_;
626  for(ElementType* r = p; r != pterm; ++r){
627  allocator.destroy(r);
628  }
629  }
630  offset = (offset + popSize) % capacity_;
631  rowSize_ -= numRows;
632  size_ -= popSize;
633  }
634 
635  void pop_front() {
636  pop_front(1);
637  }
638 
639 
640 private:
641  Allocator allocator;
642  ElementType* buf;
643  int offset;
644  int rowSize_;
645  int colSize_;
646  int capacity_;
647  int size_;
648  iterator end_;
649 };
650 
651 }
652 
653 #endif
Deque2D(const Deque2D< ElementType, Allocator > &org)
Definition: Deque2D.h:313
bool operator!=(iterator rhs) const
Definition: Deque2D.h:271
const_iterator(const const_iterator &org)
Definition: Deque2D.h:41
Column()
Definition: Deque2D.h:201
void operator++()
Definition: Deque2D.h:255
Element * end()
Definition: Deque2D.h:193
iterator(const iterator &org)
Definition: Deque2D.h:104
int size() const
Definition: Deque2D.h:219
const Element & operator[](int index) const
Definition: Deque2D.h:181
Column(const Deque2D< ElementType, Allocator > &owner, int column)
Definition: Deque2D.h:206
Definition: Deque2D.h:96
Element & at(int index)
Definition: Deque2D.h:231
Row()
Definition: Deque2D.h:157
Element * begin()
Definition: Deque2D.h:189
ElementType * term
Definition: Deque2D.h:29
void resizeRow(int newRowSize)
Definition: Deque2D.h:537
const Element & operator[](int rowIndex) const
Definition: Deque2D.h:227
const Row operator[](int rowIndex) const
Definition: Deque2D.h:569
iterator()
Definition: Deque2D.h:244
void pop_front()
Definition: Deque2D.h:635
Element & operator*()
Definition: Deque2D.h:252
bool empty() const
Definition: Deque2D.h:376
const_iterator cbegin() const
Definition: Deque2D.h:139
bool empty() const
Definition: Deque2D.h:215
iterator end()
Definition: Deque2D.h:143
const Element & at(int rowIndex, int colIndex) const
Definition: Deque2D.h:557
Row last()
Definition: Deque2D.h:581
bool empty() const
Definition: Deque2D.h:169
const Row last() const
Definition: Deque2D.h:585
void pop_back()
Definition: Deque2D.h:602
const Element & operator*() const
Definition: Deque2D.h:47
Deque2D(int rowSize, int colSize)
Definition: Deque2D.h:301
Definition: Deque2D.h:235
void resizeColumn(int newColSize)
Definition: Deque2D.h:526
void pop_front(int numRows)
Definition: Deque2D.h:606
void clear()
Definition: Deque2D.h:545
Element & operator[](int rowIndex)
Definition: Deque2D.h:223
iterator(Column &column, Element *pos)
Definition: Deque2D.h:246
iterator operator+(size_t n)
Definition: Deque2D.h:123
const_iterator(const Deque2DType &owner, ElementType *pos)
Definition: Deque2D.h:32
const Element & operator()(int rowIndex, int colIndex) const
Definition: Deque2D.h:549
ElementType * buf
Definition: Deque2D.h:30
const Column column(int colIndex) const
Definition: Deque2D.h:593
Deque2D()
Definition: Deque2D.h:292
Row(const Deque2D< ElementType, Allocator > &owner, int rowIndex)
Definition: Deque2D.h:161
void resize(int newRowSize, int newColSize)
Definition: Deque2D.h:522
Definition: Deque2D.h:16
const_iterator & operator-=(size_t n)
Definition: Deque2D.h:71
const_iterator()
Definition: Deque2D.h:39
Defines the minimum processing for performing pasing file for STL.
Definition: AbstractSceneLoader.h:9
bool operator==(iterator rhs) const
Definition: Deque2D.h:268
virtual ~Deque2D()
Definition: Deque2D.h:354
const Row row(int rowIndex) const
Definition: Deque2D.h:577
Element & operator*()
Definition: Deque2D.h:106
int size() const
Definition: Deque2D.h:173
const_iterator & operator+=(size_t n)
Definition: Deque2D.h:64
Row operator[](int rowIndex)
Definition: Deque2D.h:565
Element & operator[](int index)
Definition: Deque2D.h:177
Row row(int rowIndex)
Definition: Deque2D.h:573
Definition: Deque2D.h:23
Definition: Deque2D.h:198
int colSize() const
Definition: Deque2D.h:541
void operator--()
Definition: Deque2D.h:261
iterator begin()
Definition: Deque2D.h:135
const_iterator operator-(size_t n)
Definition: Deque2D.h:83
int rowSize() const
Definition: Deque2D.h:530
iterator & operator-=(size_t n)
Definition: Deque2D.h:116
Element & at(int rowIndex, int colIndex)
Definition: Deque2D.h:561
iterator()
Definition: Deque2D.h:103
iterator begin()
Definition: Deque2D.h:276
bool operator==(const const_iterator &rhs) const
Definition: Deque2D.h:88
bool operator!=(const const_iterator &rhs) const
Definition: Deque2D.h:91
Column column(int colIndex)
Definition: Deque2D.h:589
ElementType * current
Definition: Deque2D.h:28
iterator end()
Definition: Deque2D.h:279
const_iterator & operator++()
Definition: Deque2D.h:50
Definition: Deque2D.h:151
Deque2DType & operator=(const Deque2DType &rhs)
Definition: Deque2D.h:339
iterator & operator+=(size_t n)
Definition: Deque2D.h:109
ElementType Element
Definition: Deque2D.h:21
iterator operator-(size_t n)
Definition: Deque2D.h:128
const_iterator operator+(size_t n)
Definition: Deque2D.h:78
const_iterator & operator--()
Definition: Deque2D.h:57
Row append()
Definition: Deque2D.h:597
const_iterator cend() const
Definition: Deque2D.h:147
Element & at(int index)
Definition: Deque2D.h:185
Element & operator()(int rowIndex, int colIndex)
Definition: Deque2D.h:553