NTRT Simulator  Version: Master
 All Classes Namespaces Files Functions Variables Typedefs Friends Pages
tgStructure.cpp
Go to the documentation of this file.
1 /*
2  * Copyright © 2012, United States Government, as represented by the
3  * Administrator of the National Aeronautics and Space Administration.
4  * All rights reserved.
5  *
6  * The NASA Tensegrity Robotics Toolkit (NTRT) v1 platform is licensed
7  * under the Apache License, Version 2.0 (the "License");
8  * you may not use this file except in compliance with the License.
9  * You may obtain a copy of the License at
10  * http://www.apache.org/licenses/LICENSE-2.0.
11  *
12  * Unless required by applicable law or agreed to in writing,
13  * software distributed under the License is distributed on an
14  * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,
15  * either express or implied. See the License for the specific language
16  * governing permissions and limitations under the License.
17 */
18 
27 // This module
28 #include "tgStructure.h"
29 // This library
30 #include "tgNode.h"
31 #include "tgPair.h"
32 // The Bullet Physics library
33 #include <LinearMath/btQuaternion.h>
34 #include <LinearMath/btVector3.h>
35 
36 tgStructure::tgStructure() : tgTaggable()
37 {
38 }
39 
40 
44 tgStructure::tgStructure(const tgStructure& orig) : tgTaggable(orig.getTags()),
45  m_children(orig.m_children.size()), m_nodes(orig.m_nodes), m_pairs(orig.m_pairs)
46 {
47 
48  // Copy children
49  for (std::size_t i = 0; i < orig.m_children.size(); ++i) {
50  m_children[i] = new tgStructure(*orig.m_children[i]);
51  }
52 }
53 
54 tgStructure::tgStructure(const tgTags& tags) : tgTaggable(tags)
55 {
56 }
57 
58 tgStructure::tgStructure(const std::string& space_separated_tags) : tgTaggable(space_separated_tags)
59 {
60 }
61 
62 tgStructure::~tgStructure()
63 {
64  for (std::size_t i = 0; i < m_children.size(); ++i)
65  {
66  delete m_children[i];
67  }
68 }
69 
70 void tgStructure::addNode(double x, double y, double z, std::string tags)
71 {
72  m_nodes.addNode(x, y, z, tags);
73 }
74 
76 {
77  m_nodes.addNode(newNode);
78 }
79 
80 void tgStructure::addPair(int fromNodeIdx, int toNodeIdx, std::string tags)
81 {
82  addPair(m_nodes[fromNodeIdx], m_nodes[toNodeIdx], tags);
83 }
84 
85 void tgStructure::addPair(const btVector3& from, const btVector3& to, std::string tags)
86 {
87  // @todo: do we need to pass in tags here? might be able to save some proc time if not...
88  tgPair p = tgPair(from, to);
89  if (!m_pairs.contains(p))
90  {
91  m_pairs.addPair(tgPair(from, to, tags));
92  }
93  else
94  {
95  std::ostringstream os;
96  os << "A pair matching " << p << " already exists in this structure.";
97  throw tgException(os.str());
98  }
99 }
100 
101 void tgStructure::removePair(const tgPair& pair) {
102  m_pairs.removePair(pair);
103  for (unsigned int i = 0; i < m_children.size(); i++) {
104  m_children[i]->removePair(pair);
105  }
106 }
107 
108 void tgStructure::move(const btVector3& offset)
109 {
110  m_nodes.move(offset);
111  m_pairs.move(offset);
112  for (size_t i = 0; i < m_children.size(); ++i)
113  {
114  tgStructure * const pStructure = m_children[i];
115  assert(pStructure != NULL);
116  pStructure->move(offset);
117  }
118 }
119 
120 void tgStructure::addRotation(const btVector3& fixedPoint,
121  const btVector3& axis,
122  double angle)
123 {
124  const btQuaternion rotation(axis, angle);
125  addRotation(fixedPoint, rotation);
126 }
127 
128 void tgStructure::addRotation(const btVector3& fixedPoint,
129  const btVector3& fromOrientation,
130  const btVector3& toOrientation)
131 {
132  addRotation(fixedPoint,
133  tgUtil::getQuaternionBetween(fromOrientation,
134  toOrientation));
135 }
136 
137 void tgStructure::addRotation(const btVector3& fixedPoint,
138  const btQuaternion& rotation)
139 {
140  m_nodes.addRotation(fixedPoint, rotation);
141  m_pairs.addRotation(fixedPoint, rotation);
142 
143  for (std::size_t i = 0; i < m_children.size(); ++i)
144  {
145  tgStructure * const pStructure = m_children[i];
146  assert(pStructure != NULL);
147  pStructure->addRotation(fixedPoint, rotation);
148  }
149 }
150 
151 void tgStructure::scale(double scaleFactor) {
152  btVector3 structureCentroid = getCentroid();
153  scale(structureCentroid, scaleFactor);
154 }
155 
156 void tgStructure::scale(const btVector3& referencePoint, double scaleFactor) {
157  m_nodes.scale(referencePoint, scaleFactor);
158  m_pairs.scale(referencePoint, scaleFactor);
159 
160  for (int i = 0; i < m_children.size(); i++) {
161  tgStructure* const childStructure = m_children[i];
162  assert(childStructure != NULL);
163  childStructure->scale(referencePoint, scaleFactor);
164  }
165 }
166 
168 {
173  if (pChild != NULL)
174  {
175  m_children.push_back(pChild);
176  }
177 }
178 
179 void tgStructure::addChild(const tgStructure& child)
180 {
181  m_children.push_back(new tgStructure(child));
182 
183 }
184 
185 btVector3 tgStructure::getCentroid() const {
186  btVector3 centroid = btVector3(0, 0, 0);
187  int numNodes = 0;
188  std::queue< const tgStructure*> q;
189 
190  q.push(this);
191 
192  while (!q.empty()) {
193  const tgStructure* structure = q.front();
194  q.pop();
195  for (int i = 0; i < structure->m_nodes.size(); i++) {
196  centroid += structure->m_nodes[i];
197  numNodes++;
198  }
199  for (int i = 0; i < structure->m_children.size(); i++) {
200  q.push(structure->m_children[i]);
201  }
202  }
203  return centroid/numNodes;
204 }
205 
206 tgNode& tgStructure::findNode(const std::string& tags) {
207  std::queue<tgStructure*> q;
208 
209  q.push(this);
210 
211  while (!q.empty()) {
212  tgStructure* structure = q.front();
213  q.pop();
214  for (int i = 0; i < structure->m_nodes.size(); i++) {
215  if (structure->m_nodes[i].hasAllTags(tags)) {
216  return structure->m_nodes[i];
217  }
218  }
219  for (int i = 0; i < structure->m_children.size(); i++) {
220  q.push(structure->m_children[i]);
221  }
222  }
223  throw std::invalid_argument("Node not found: " + tags);
224 }
225 
226 tgPair& tgStructure::findPair(const btVector3& from, const btVector3& to) {
227  std::queue<tgStructure*> q;
228 
229  q.push(this);
230 
231  while (!q.empty()) {
232  tgStructure* structure = q.front();
233  q.pop();
234  for (int i = 0; i < structure->m_pairs.size(); i++) {
235  if ((structure->m_pairs[i].getFrom() == from && structure->m_pairs[i].getTo() == to) ||
236  (structure->m_pairs[i].getFrom() == to && structure->m_pairs[i].getTo() == from)) {
237  return structure->m_pairs[i];
238  }
239  }
240  for (int i = 0; i < structure->m_children.size(); i++) {
241  q.push(structure->m_children[i]);
242  }
243  }
244  std::ostringstream pairString;
245  pairString << from << ", " << to;
246  throw std::invalid_argument("Pair not found: " + pairString.str());
247 }
248 
249 tgStructure& tgStructure::findChild(const std::string& tags) {
250  std::queue<tgStructure*> q;
251 
252  for (int i = 0; i < m_children.size(); i++) {
253  q.push(m_children[i]);
254  }
255 
256  while (!q.empty()) {
257  tgStructure* structure = q.front();
258  q.pop();
259  if (structure->hasAllTags(tags)) {
260  return *structure;
261  }
262  for (int i = 0; i < structure->m_children.size(); i++) {
263  q.push(structure->m_children[i]);
264  }
265  }
266  throw std::invalid_argument("Child structure not found: " + tags);
267 }
268 
269 /* Standalone functions */
270 std::string asYamlElement(const tgStructure& structure, int indentLevel)
271 {
272  std::stringstream os;
273  std::string indent = std::string(2 * (indentLevel), ' ');
274  os << indent << "structure:" << std::endl;
275  os << indent << " tags: " << asYamlList(structure.getTags()) << std::endl;
276  os << asYamlItems(structure.getNodes(), indentLevel + 1);
277  os << asYamlItems(structure.getPairs(), indentLevel + 1);
278  os << asYamlItems(structure.getChildren(), indentLevel + 1);
279  return os.str();
280 }
281 
282 std::string asYamlItem(const tgStructure& structure, int indentLevel)
283 {
284  std::stringstream os;
285  std::string indent = std::string(2 * (indentLevel), ' ');
286  os << indent << "- tags: " << asYamlList(structure.getTags()) << std::endl;
287  os << asYamlItems(structure.getNodes(), indentLevel + 1);
288  os << asYamlItems(structure.getPairs(), indentLevel + 1);
289  os << asYamlItems(structure.getChildren(), indentLevel + 1);
290  return os.str();
291 }
292 
293 std::string asYamlItems(const std::vector<tgStructure*> structures, int indentLevel)
294 {
295  std::stringstream os;
296  std::string indent = std::string(2 * (indentLevel), ' ');
297  if (structures.size() == 0) {
298  os << indent << "structures: []" << std::endl;
299  return os.str();
300  }
301 
302  os << indent << "structures:" << std::endl;
303  for(size_t i = 0; i < structures.size(); i++)
304  {
305  os << asYamlItem(*structures[i], indentLevel+1);
306  }
307  return os.str();
308 }
std::string asYamlList(const tgTags &tags)
Definition: tgTags.h:356
std::string asYamlElement(const tgStructure &structure, int indentLevel)
tgStructure & findChild(const std::string &name)
static btQuaternion getQuaternionBetween(btVector3 a, btVector3 b, const btVector3 &fallbackAxis=btVector3(0, 0, 0))
Definition: tgUtil.h:199
tgNode & findNode(const std::string &name)
const std::vector< tgStructure * > & getChildren() const
Definition: tgStructure.h:184
void addChild(tgStructure *child)
Definition of class tgPair.
btVector3 getCentroid() const
const tgNodes & getNodes() const
Definition: tgStructure.h:138
void addPair(int fromNodeIdx, int toNodeIdx, std::string tags="")
Definition: tgStructure.cpp:80
const tgPairs & getPairs() const
Definition: tgStructure.h:165
void addRotation(const btVector3 &fixedPoint, const btVector3 &axis, double angle)
Definition of class tgNode.
Definition: tgPair.h:48
Definition: tgNode.h:45
Definition of class tgStructure.
tgPair & findPair(const btVector3 &from, const btVector3 &to)
void move(const btVector3 &offset)
Definition: tgNodes.h:175
std::string asYamlItem(const tgStructure &structure, int indentLevel)
std::string asYamlItems(const std::vector< tgStructure * > structures, int indentLevel)
int addNode(const btVector3 &node)
Definition: tgNodes.h:129
Definition: tgTags.h:44
void addNode(double x, double y, double z, std::string tags="")
Definition: tgStructure.cpp:70