NTRT Simulator  Version: Master
 All Classes Namespaces Files Functions Variables Typedefs Friends Pages
tgCompoundRigidInfo.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 
26 #include "tgCompoundRigidInfo.h"
27 
29 {
30 }
31 
32 tgModel* tgCompoundRigidInfo::createModel(tgWorld& world)
33 {
34  // @todo: make tgModel a true composite and use that here
35  tgModel* result = new tgModel();
36  for(int i = 0; i < m_rigids.size(); i++) {
37  result->addChild(m_rigids[i]->createModel(world));
38  }
39  return result;
40 }
41 
43 {
44  m_rigids.push_back(&rigid);
45 }
46 
48 {
49  btVector3 sum = btVector3(0.0, 0.0, 0.0);
50  for (int ii = 0; ii < m_rigids.size(); ii++)
51  {
52  /* const */ tgRigidInfo * const rigid = m_rigids[ii];
53  sum += (rigid->getCenterOfMass() * rigid->getMass());
54  }
55  const double totalMass = getMass();
56  return
57  (totalMass == 0.0) ? btVector3(0.0, 0.0, 0.0) : (sum / totalMass);
58 }
59 
60 
61 btCompoundShape* tgCompoundRigidInfo::createCompoundShape(tgWorld& world) const
62 {
63  if (m_compoundShape == 0)
64  {
65  // Deallocated by the world implementation
66  m_compoundShape = new btCompoundShape(&world);
67 
68  const btVector3 com = getCenterOfMass();
69 
70  for (int ii = 0; ii < m_rigids.size(); ii++)
71  {
72  tgRigidInfo* const rigid = m_rigids[ii];
73  btTransform t = rigid->getTransform();
74  t.setOrigin(t.getOrigin() - com);
75  m_compoundShape->addChildShape(t, rigid->getCollisionShape(world));
76  }
77  // Add the collision shape to the array so we can delete it later
78  tgWorldBulletPhysicsImpl& bulletWorld =
81 
82  }
83  return m_compoundShape;
84 }
85 
86 btCollisionShape* tgCompoundRigidInfo::getCollisionShape(tgWorld& world) const
87 {
88  if (m_compoundShape == 0)
89  {
91  }
92  return m_compoundShape;
93 }
94 
96 {
97  btTransform t;
98  t.setIdentity();
99  t.setOrigin(getCenterOfMass());
100  return t;
101 }
102 
104 {
106  double mass = 0.0;
107  for (int ii = 0; ii < m_rigids.size(); ii++)
108  {
109  mass += m_rigids[ii]->getMass();
110  }
111  return mass;
112 }
113 
115 {
116  btRigidBody* rb = tgCast::cast<btCollisionObject, btRigidBody>(m_collisionObject);
117  return rb;
118 }
119 
120 const btRigidBody* tgCompoundRigidInfo::getRigidBody() const
121 {
122  btRigidBody* rb = tgCast::cast<btCollisionObject, btRigidBody>(m_collisionObject);
123  return rb;
124 }
125 
126 void tgCompoundRigidInfo::setRigidBody(btRigidBody* const rigidBody)
127 {
128  m_collisionObject = rigidBody;
129  // Set the rigid body for all components
131  for (int ii = 0; ii < m_rigids.size(); ii++) {
132  m_rigids[ii]->setRigidBody(rigidBody);
133  }
134 }
135 
136 void tgCompoundRigidInfo::setCollisionObject(btCollisionObject* collisionObject)
137 {
138  m_collisionObject = collisionObject;
139  // Set the rigid body for all components
141  for (int ii = 0; ii < m_rigids.size(); ii++) {
142  m_rigids[ii]->setCollisionObject(collisionObject);
143  }
144 }
145 
146 
147 std::set<tgRigidInfo*> tgCompoundRigidInfo::getLeafRigids()
148 {
149  std::set<tgRigidInfo*> leaves;
150  for (int ii = 0; ii < m_rigids.size(); ii++) {
151  tgRigidInfo * const rigid = m_rigids[ii];
152  if (rigid->isCompound())
153  {
154  // Insert the leaves from the compound recursively
155  const std::set<tgRigidInfo*> compoundLeaves =
156  rigid->getLeafRigids();
157  leaves.insert(compoundLeaves.begin(), compoundLeaves.end());
158  }
159  else
160  {
161  leaves.insert(rigid);
162  }
163  }
164  return leaves;
165 }
166 
167 bool tgCompoundRigidInfo::containsNode(const btVector3& nodeVector) const
168 {
170  for (int ii = 0; ii < m_rigids.size(); ii++)
171  {
172  if (m_rigids[ii]->containsNode(nodeVector))
173  {
174  return true;
175  }
176  }
177  return false;
178 }
179 
181 {
183  for (int ii = 0; ii < m_rigids.size(); ii++)
184  {
185  if (m_rigids[ii]->sharesNodesWith(other)) {
186  return true;
187  }
188  }
189  return false;
190 }
191 
192 std::set<btVector3> tgCompoundRigidInfo::getContainedNodes() const
193 {
195  std::set<btVector3> contained;
196  for (int ii = 0; ii < m_rigids.size(); ii++)
197 {
198  const std::set<btVector3> nodes = m_rigids[ii]->getContainedNodes();
199  contained.insert(nodes.begin(), nodes.end());
200  }
201  return contained;
202 }
203 
bool isCompound() const
Definition: tgRigidInfo.h:273
std::vector< tgRigidInfo * > m_rigids
virtual bool containsNode(const btVector3 &nodeVector) const
void addChild(tgModel *pChild)
Definition: tgModel.cpp:122
virtual btRigidBody * getRigidBody()
virtual bool sharesNodesWith(const tgRigidInfo &other) const
Definition of class tgCompoundRigidInfo.
virtual double getMass() const
virtual btCollisionShape * getCollisionShape(tgWorld &world) const =0
virtual std::set< tgRigidInfo * > getLeafRigids()=0
std::set< btVector3 > getContainedNodes() const
virtual btVector3 getCenterOfMass() const
virtual void setCollisionObject(btCollisionObject *collisionObject)
virtual btTransform getTransform() const
btCollisionObject * m_collisionObject
Definition: tgRigidInfo.h:347
virtual btVector3 getCenterOfMass() const =0
tgWorldImpl & implementation() const
Definition: tgWorld.h:108
virtual void setRigidBody(btRigidBody *const rigidBody)
virtual double getMass() const =0
virtual std::set< tgRigidInfo * > getLeafRigids()
void addRigid(tgRigidInfo &rigid)
void addCollisionShape(btCollisionShape *pShape)
virtual btTransform getTransform() const =0
btCompoundShape * m_compoundShape
virtual btCollisionShape * getCollisionShape(tgWorld &world) const
btCompoundShape * createCompoundShape(tgWorld &world) const