DART  6.10.1
DegreeOfFreedomPtr.hpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2011-2021, The DART development contributors
3  * All rights reserved.
4  *
5  * The list of contributors can be found at:
6  * https://github.com/dartsim/dart/blob/master/LICENSE
7  *
8  * This file is provided under the following "BSD-style" License:
9  * Redistribution and use in source and binary forms, with or
10  * without modification, are permitted provided that the following
11  * conditions are met:
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
19  * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
20  * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
21  * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
23  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
24  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
25  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
26  * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  */
32 
33 #ifndef DART_DYNAMICS_DETAIL_DEGREEOFFREEDOMPTR_HPP_
34 #define DART_DYNAMICS_DETAIL_DEGREEOFFREEDOMPTR_HPP_
35 
38 
39 namespace dart {
40 namespace dynamics {
41 
51 template <class DegreeOfFreedomT, class BodyNodeT>
53 {
54 public:
55  template <class, class>
57 
60 
63  TemplateDegreeOfFreedomPtr(DegreeOfFreedomT* _ptr)
64  {
65  set(_ptr);
66  }
67 
69  template <class OtherDegreeOfFreedomT, class OtherBodyNodeT>
72  _dofp)
73  {
74  set(_dofp.get());
75  }
76 
78  TemplateDegreeOfFreedomPtr& operator=(DegreeOfFreedomT* _ptr)
79  {
80  set(_ptr);
81  return *this;
82  }
83 
85  template <class OtherDegreeOfFreedomT, class OtherBodyNodeT>
88  _dofp)
89  {
90  set(_dofp.get());
91  return *this;
92  }
93 
95  operator DegreeOfFreedomT*() const
96  {
97  return get();
98  }
99 
101  DegreeOfFreedomT& operator*() const
102  {
103  return *get();
104  }
105 
107  DegreeOfFreedomT* operator->() const
108  {
109  return get();
110  }
111 
113  DegreeOfFreedomT* get() const
114  {
115  if (nullptr == mBodyNodePtr)
116  return nullptr;
117 
118  return mBodyNodePtr->getParentJoint()->getDof(mIndex);
119  }
120 
123  {
124  return mBodyNodePtr;
125  }
126 
129  std::size_t getLocalIndex() const
130  {
131  if (nullptr == mBodyNodePtr)
132  return INVALID_INDEX;
133 
134  return mIndex;
135  }
136 
138  void set(DegreeOfFreedomT* _ptr)
139  {
140  if (nullptr == _ptr)
141  {
142  mBodyNodePtr = nullptr;
143  return;
144  }
145 
146  mBodyNodePtr = _ptr->getChildBodyNode();
147  mIndex = _ptr->getIndexInJoint();
148  }
149 
150  //----------------------------------------------------------------------------
152  //----------------------------------------------------------------------------
153 
155  template <class OtherDofT, class OtherBodyNodeT>
158  {
159  if (nullptr == mBodyNodePtr && nullptr == _rhs.mBodyNodePtr)
160  return true;
161 
162  if ((mBodyNodePtr == _rhs.mBodyNodePtr) && (mIndex == _rhs.mIndex))
163  return true;
164 
165  return false;
166  }
167 
169  template <class OtherDofT, class OtherBodyNodeT>
172  {
173  return !(*this == _rhs);
174  }
175 
177  template <class OtherDofT, class OtherBodyNodeT>
178  bool operator<(
180  {
181  if (mBodyNodePtr == _rhs.mBodyNodePtr)
182  return (mIndex < _rhs.mIndex);
183 
184  return (mBodyNodePtr < _rhs.mBodyNodePtr);
185  }
186 
188  template <class OtherDofT, class OtherBodyNodeT>
189  bool operator>(
191  {
192  if (mBodyNodePtr == _rhs.mBodyNodePtr)
193  return (mIndex > _rhs.mIndex);
194 
195  return (mBodyNodePtr > _rhs.mBodyNodePtr);
196  }
197 
199  template <class OtherDofT, class OtherBodyNodeT>
202  {
203  return (*this < _rhs) || (*this == _rhs);
204  }
205 
207  template <class OtherDofT, class OtherBodyNodeT>
210  {
211  return (*this > _rhs) || (*this == _rhs);
212  }
213 
215 
216 private:
219 
221  std::size_t mIndex;
222 };
223 
230 template <class DegreeOfFreedomT, class BodyNodeT>
232 {
233 public:
234  template <class, class>
236 
239  {
240  set(nullptr);
241  }
242 
245  TemplateWeakDegreeOfFreedomPtr(DegreeOfFreedomT* _ptr)
246  {
247  set(_ptr);
248  }
249 
251  template <class OtherDofT, class OtherBodyNodeT>
254  {
255  set(_weakPtr);
256  }
257 
259  template <class OtherDofT, class OtherBodyNodeT>
262  {
263  set(_strongPtr.get());
264  }
265 
267  TemplateWeakDegreeOfFreedomPtr& operator=(DegreeOfFreedomT* _ptr)
268  {
269  set(_ptr);
270  return *this;
271  }
272 
274  template <class OtherDofT, class OtherBodyNodeT>
277  {
278  set(_weakPtr);
279  return *this;
280  }
281 
283  template <class OtherDofT, class OtherBodyNodeT>
286  {
287  set(_strongPtr.get());
288  return *this;
289  }
290 
296  {
298  if (nullptr == bodyNode)
299  return nullptr;
300 
302  bodyNode->getParentJoint()->getDof(mIndex));
303  }
304 
306  void set(DegreeOfFreedomT* _ptr)
307  {
308  if (nullptr == _ptr)
309  {
310  mWeakBodyNode = nullptr;
311  mIndex = 0;
312  return;
313  }
314 
315  mWeakBodyNode = _ptr->getChildBodyNode();
316  mIndex = _ptr->getIndexInJoint();
317  }
318 
321  template <class OtherDofT, class OtherBodyNodeT>
322  void set(
324  {
325  mWeakBodyNode = _weakPtr.mWeakBodyNode;
326  mIndex = _weakPtr.mIndex;
327  }
328 
329 private:
332 
334  std::size_t mIndex;
335 };
336 
337 } // namespace dynamics
338 } // namespace dart
339 
340 #endif // DART_DYNAMICS_DETAIL_DEGREEOFFREEDOMPTR_HPP_
TemplateBodyNodePtr is a templated class that enables users to create a reference-counting BodyNodePt...
Definition: BodyNodePtr.hpp:110
TemplateDegreeOfFreedomPtr is a templated class that enables users to create a reference-counting Deg...
Definition: DegreeOfFreedomPtr.hpp:53
DegreeOfFreedomT & operator*() const
Dereferencing operator.
Definition: DegreeOfFreedomPtr.hpp:101
TemplateBodyNodePtr< BodyNodeT > mBodyNodePtr
Reference-holding pointer to the child BodyNode of this DegreeOfFreedom.
Definition: DegreeOfFreedomPtr.hpp:218
bool operator>=(const TemplateDegreeOfFreedomPtr< OtherDofT, OtherBodyNodeT > &_rhs) const
Greater than or equal to.
Definition: DegreeOfFreedomPtr.hpp:208
bool operator<=(const TemplateDegreeOfFreedomPtr< OtherDofT, OtherBodyNodeT > &_rhs) const
Less than or equal to.
Definition: DegreeOfFreedomPtr.hpp:200
bool operator==(const TemplateDegreeOfFreedomPtr< OtherDofT, OtherBodyNodeT > &_rhs) const
Equality.
Definition: DegreeOfFreedomPtr.hpp:156
bool operator>(const TemplateDegreeOfFreedomPtr< OtherDofT, OtherBodyNodeT > &_rhs) const
Greater than.
Definition: DegreeOfFreedomPtr.hpp:189
TemplateDegreeOfFreedomPtr()=default
Default constructor.
TemplateDegreeOfFreedomPtr(DegreeOfFreedomT *_ptr)
Typical constructor.
Definition: DegreeOfFreedomPtr.hpp:63
TemplateDegreeOfFreedomPtr(const TemplateDegreeOfFreedomPtr< OtherDegreeOfFreedomT, OtherBodyNodeT > &_dofp)
Constructor that takes in a strong DegreeOfFreedomPtrs.
Definition: DegreeOfFreedomPtr.hpp:70
DegreeOfFreedomT * operator->() const
Dereferencing operation.
Definition: DegreeOfFreedomPtr.hpp:107
TemplateDegreeOfFreedomPtr & operator=(DegreeOfFreedomT *_ptr)
Assignment operator.
Definition: DegreeOfFreedomPtr.hpp:78
TemplateBodyNodePtr< BodyNodeT > getBodyNodePtr() const
Get the BodyNode that this DegreeOfFreedomPtr is tied to.
Definition: DegreeOfFreedomPtr.hpp:122
void set(DegreeOfFreedomT *_ptr)
Set the DegreeOfFreedom for this DegreeOfFreedomPtr.
Definition: DegreeOfFreedomPtr.hpp:138
bool operator!=(const TemplateDegreeOfFreedomPtr< OtherDofT, OtherBodyNodeT > &_rhs) const
Inequality.
Definition: DegreeOfFreedomPtr.hpp:170
bool operator<(const TemplateDegreeOfFreedomPtr< OtherDofT, OtherBodyNodeT > &_rhs) const
Less than.
Definition: DegreeOfFreedomPtr.hpp:178
TemplateDegreeOfFreedomPtr & operator=(const TemplateDegreeOfFreedomPtr< OtherDegreeOfFreedomT, OtherBodyNodeT > &_dofp)
Assignment operator for DegreeOfFreedomPtrs.
Definition: DegreeOfFreedomPtr.hpp:86
std::size_t getLocalIndex() const
Get the local generalized coordinate index that this DegreeOfFreedomPtr is tied to.
Definition: DegreeOfFreedomPtr.hpp:129
DegreeOfFreedomT * get() const
Get the raw DegreeOfFreedom pointer.
Definition: DegreeOfFreedomPtr.hpp:113
std::size_t mIndex
Local index of this DegreeOfFreedom within its Joint.
Definition: DegreeOfFreedomPtr.hpp:221
TemplateWeakBodyNodePtr is a templated class that enables users to create a non-reference-holding Wea...
Definition: BodyNodePtr.hpp:224
TemplateWeakDegreeOfFreedomPtr is a templated class that enables users to create a non-reference-hold...
Definition: DegreeOfFreedomPtr.hpp:232
void set(DegreeOfFreedomT *_ptr)
Set the DegreeOfFreedom for this WeakDegreeOfFreedomPtr.
Definition: DegreeOfFreedomPtr.hpp:306
TemplateWeakDegreeOfFreedomPtr & operator=(const TemplateWeakDegreeOfFreedomPtr< OtherDofT, OtherBodyNodeT > &_weakPtr)
Assignemnt operator for WeakDegreeOfFreedomPtrs.
Definition: DegreeOfFreedomPtr.hpp:275
TemplateWeakDegreeOfFreedomPtr(const TemplateWeakDegreeOfFreedomPtr< OtherDofT, OtherBodyNodeT > &_weakPtr)
Constructor that takes in a WeakDegreeOfFreedomPtr.
Definition: DegreeOfFreedomPtr.hpp:252
TemplateWeakDegreeOfFreedomPtr(const TemplateDegreeOfFreedomPtr< OtherDofT, OtherBodyNodeT > &_strongPtr)
Constructor that takes in a strong DegreeOfFreedomPtr.
Definition: DegreeOfFreedomPtr.hpp:260
TemplateWeakDegreeOfFreedomPtr()
Default constructor.
Definition: DegreeOfFreedomPtr.hpp:238
TemplateWeakDegreeOfFreedomPtr & operator=(const TemplateDegreeOfFreedomPtr< OtherDofT, OtherBodyNodeT > &_strongPtr)
Assignment operator for strong DegreeOfFreedomPtrs.
Definition: DegreeOfFreedomPtr.hpp:284
TemplateWeakBodyNodePtr< BodyNodeT > mWeakBodyNode
Weak pointer to the child BodyNode of this DegreeOfFreedom.
Definition: DegreeOfFreedomPtr.hpp:331
TemplateWeakDegreeOfFreedomPtr(DegreeOfFreedomT *_ptr)
Typical constructor.
Definition: DegreeOfFreedomPtr.hpp:245
void set(const TemplateWeakDegreeOfFreedomPtr< OtherDofT, OtherBodyNodeT > &_weakPtr)
Attempt to set the DegreeOfFreedom for this WeakDegreeOfFreedomPtr based on another WeakDegreeOfFreed...
Definition: DegreeOfFreedomPtr.hpp:322
TemplateWeakDegreeOfFreedomPtr & operator=(DegreeOfFreedomT *_ptr)
Assignment operator for raw DegreeOfFreedom pointers.
Definition: DegreeOfFreedomPtr.hpp:267
std::size_t mIndex
Local index of this DegreeOfFreedom within its Joint.
Definition: DegreeOfFreedomPtr.hpp:334
TemplateDegreeOfFreedomPtr< DegreeOfFreedomT, BodyNodeT > lock() const
Locks the DegreeOfFreedom reference to ensure that the referenced DegreeOfFreedom is currently still ...
Definition: DegreeOfFreedomPtr.hpp:295
constexpr std::size_t INVALID_INDEX
Definition: InvalidIndex.hpp:41
Definition: BulletCollisionDetector.cpp:65