NTRT Simulator  v1.1
 All Classes Namespaces Files Functions Variables Typedefs Friends Pages
tgRodInfo.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 "tgRodInfo.h"
29 
30 // The NTRT Core library
32 
33 // The Bullet Physics Library
34 #include "LinearMath/btVector3.h"
35 
36 // The C++ Standard Library
37 #include <algorithm>
38 #include <stdexcept>
39 #include <cassert>
40 
41 // @todo: Need to take tags into account...
42 
44  m_pair(),
45  m_config(config),
46  tgRigidInfo()
47 {}
48 
49 tgRodInfo::tgRodInfo(const tgRod::Config& config, tgTags tags) :
50  m_pair(),
51  m_config(config),
52  tgRigidInfo(tags)
53 {}
54 
55 tgRodInfo::tgRodInfo(const tgRod::Config& config, const tgPair& pair) :
56  m_pair(pair),
57  m_config(config),
58  tgRigidInfo(pair.getTags())
59 {}
60 
61 tgRodInfo::tgRodInfo(const tgRod::Config& config, tgTags tags, const tgPair& pair) :
62  m_pair(pair),
63  m_config(config),
64  tgRigidInfo( tags + pair.getTags() )
65 {}
66 
68 {
69  return new tgRodInfo(m_config, pair);
70 }
71 
73 {
74  tgRigidInfo::initRigidBody(world);
75  assert(m_collisionObject != NULL);
76  getRigidBody()->setFriction(m_config.friction);
77  getRigidBody()->setRollingFriction(m_config.rollFriction);
78  getRigidBody()->setRestitution(m_config.restitution);
79 }
80 
81 tgModel* tgRodInfo::createModel(tgWorld& world)
82 {
83  // @todo: handle tags -> model
84  // @todo: check to make sure the rigidBody has been built
85  // @todo: Do we need to use the group here?
86 
87  // Just in case it hasn't been done already...
88  initRigidBody(world);
89 
90  #if (0)
91  std::cout << "creating rod with tags " << getTags() << std::endl;
92  #endif
93 
94  tgRod* rod = new tgRod(getRigidBody(), getTags(), getLength());
95 
96  return rod;
97 }
98 
99 btCollisionShape* tgRodInfo::getCollisionShape(tgWorld& world) const
100 {
101  if (m_collisionShape == NULL)
102  {
103  const double radius = m_config.radius;
104  const double length = getLength();
106  new btCylinderShape(btVector3(radius, length / 2.0, radius));
107 
108  // Add the collision shape to the array so we can delete it later
109  tgWorldBulletPhysicsImpl& bulletWorld =
111  bulletWorld.addCollisionShape(m_collisionShape);
112  }
113  return m_collisionShape;
114 }
115 
116 double tgRodInfo::getMass() const
117 {
118  const double length = getLength();
119  const double radius = m_config.radius;
120  const double density = m_config.density;
121  const double volume = M_PI * radius * radius * length;
122  return volume * density;
123 }
124 
125 btVector3
126 tgRodInfo::getConnectionPoint(const btVector3& referencePoint,
127  const btVector3& destinationPoint) const
128 {
129  return getConnectionPoint(referencePoint, destinationPoint, 0);
130 }
131 
132 btVector3
133 tgRodInfo::getConnectionPoint(const btVector3& referencePoint,
134  const btVector3& destinationPoint,
135  const double rotation) const
136 {
137  if (referencePoint == destinationPoint)
138  {
139  throw
140  std::invalid_argument("Destination point is the reference point.");
141  }
142  // Find the closest point on the radius from the referencePoint
143  const btVector3 cylinderAxis = (getTo() - getFrom()).normalize();
144  const btVector3 cylinderAxis2 = (getTo() - getFrom()).normalize();
145  // Vector from reference point to destination point
146  const btVector3 refToDest =
147  (referencePoint - destinationPoint).normalize();
148 
149  // Find a vector perpendicular to both the cylinder axis and refToDest
150  btVector3 rotationAxis = cylinderAxis.cross(refToDest);
151 
152  // Handle a vector crossed with itself
153  if (rotationAxis.length() == 0.0)
154  {
155  btScalar a = cylinderAxis[0];
156  btScalar b = cylinderAxis[1];
157  btScalar c = cylinderAxis[2];
158  // Find an arbitrary perpendicular vector
159  rotationAxis = btVector3(b - c, -a, a).normalize();
160  }
161 
162  const btVector3 directional =
163  cylinderAxis.rotate(rotationAxis, -M_PI / 2.0).normalize();
164 
165  // Apply one additional rotation so we can end up anywhere we
166  // want on the radius of the rod
167 
168  // When added to any point along the cylinder axis, this will take you
169  // to the surface in the direction of the destinationPoint
170 
171  const btVector3 surfaceVector = directional.rotate(cylinderAxis2, rotation).normalize()
172  * m_config.radius;
173 
174  // Return the the surface point closest to the reference point in the
175  // direction of the destination point.
176  return referencePoint + surfaceVector;
177 }
178 
179 std::set<tgRigidInfo*> tgRodInfo::getLeafRigids()
180 {
181  std::set<tgRigidInfo*> leaves;
182  leaves.insert(this);
183  return leaves;
184 }
185 
186 std::set<btVector3> tgRodInfo::getContainedNodes() const {
187  std::set<btVector3> contained;
188  contained.insert(getFrom());
189  contained.insert(getTo());
190  return contained;
191 }
const btVector3 & getFrom() const
Definition: tgRodInfo.h:110
virtual void initRigidBody(tgWorld &world)
Definition: tgRodInfo.cpp:72
const double rollFriction
Definition: tgRod.h:80
Definition of class tgRodInfo.
tgRodInfo(const tgRod::Config &config)
Definition: tgRodInfo.cpp:43
virtual btCollisionShape * getCollisionShape(tgWorld &world) const
Definition: tgRodInfo.cpp:99
Contains the definition of class tgWorldBulletPhysicsImpl.
virtual std::set< tgRigidInfo * > getLeafRigids()
Definition: tgRodInfo.cpp:179
const double density
Definition: tgRod.h:71
tgRigidInfo * createRigidInfo(const tgPair &pair)
Definition: tgRodInfo.cpp:67
const double radius
Definition: tgRod.h:68
virtual std::set< btVector3 > getContainedNodes() const
Definition: tgRodInfo.cpp:186
double getLength() const
Definition: tgRodInfo.h:213
virtual btVector3 getConnectionPoint(const btVector3 &referencePoint, const btVector3 &destinationPoint) const
Definition: tgRodInfo.cpp:126
virtual btRigidBody * getRigidBody()
btCollisionObject * m_collisionObject
Definition: tgRigidInfo.h:347
tgWorldImpl & implementation() const
Definition: tgWorld.h:102
Definition: tgPair.h:48
const double friction
Definition: tgRod.h:76
const btVector3 & getTo() const
Definition: tgRodInfo.h:113
void addCollisionShape(btCollisionShape *pShape)
const double restitution
Definition: tgRod.h:84
virtual double getMass() const
Definition: tgRodInfo.cpp:116
Definition: tgRod.h:43
Definition: tgTags.h:44
btCollisionShape * m_collisionShape
Definition: tgRigidInfo.h:332