NTRT Simulator  Version: Master
 All Classes Namespaces Files Functions Variables Typedefs Friends Pages
tgBoxMoreAnchorsInfo.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 "tgBoxMoreAnchorsInfo.h"
29 
30 // The NTRT Core library
32 
33 // The Bullet Physics Library
34 #include "LinearMath/btVector3.h"
35 #include "BulletCollision/CollisionShapes/btBoxShape.h"
36 
37 // The C++ Standard Library
38 #include <algorithm>
39 #include <stdexcept>
40 #include <cassert>
41 
42 // The four constructors. Each just calls the parent's constructor.
44  tgBoxInfo(config)
45 {}
46 
48  tgTags tags) :
49  tgBoxInfo(config, tags)
50 {}
51 
53  const tgPair& pair) :
54  tgBoxInfo(config, pair)
55 {}
56 
58  tgTags tags, const tgPair& pair) :
59  tgBoxInfo(config, tags, pair)
60 {}
61 
68 {
69  return new tgBoxMoreAnchorsInfo(getConfig(), pair);
70 }
71 
72 // Create and return a tgBoxMoreAnchors.
74 {
75  // @todo: handle tags -> model
76  // @todo: check to make sure the rigidBody has been built
77  // @todo: Do we need to use the group here?
78 
79  // Just in case it hasn't been done already...
80  initRigidBody(world);
81 
82  #if (0)
83  std::cout << "creating tgBoxMoreAnchors with tags " << getTags() << std::endl;
84  #endif
85 
87  getTags(), getLength());
88 
89  return box;
90 }
91 
92 // Redefine the parent class' method so that a tgBoxMoreAnchorsInfo is returned.
93 std::set<tgRigidInfo*> tgBoxMoreAnchorsInfo::getLeafRigids()
94 {
95  std::set<tgRigidInfo*> leaves;
96  leaves.insert(this);
97  return leaves;
98 }
99 
110  // The return value will be a string:
111  std::string boxOrientation;
112  // Determine box orientation by dotting the vector that runs
113  // along the length of the box with (1,0,0), (0,1,0), and (0,0,1).
114  btVector3 lengthVector = getTo() - getFrom();
115  //DEBUGGING:
116  //std::cout << "Inside tgBoxMoreAnchorsInfo::getBoxOrientation, lengthVector is "
117  // << lengthVector << "." << std::endl;
118 
119  // For now, check to be sure the box is orthogonal to the axes of the world.
120  // This class does not currently support boxes at an angle.
121  // Only one of the dimensions in lengthVector should be nonzero.
122  if( lengthVector.dot( btVector3(1, 0, 0)) != 0.0 ) {
123  // There is a nonzero in x.
124  // Check if another nonzero:
125  if( lengthVector.dot( btVector3(0, 1, 0)) != 0.0 ) {
126  // Box is not aligned along an axis, no prescription for such a case yet.
127  throw std::invalid_argument("tgBoxMoreAnchors is not aligned along an axis! This class currently only supports boxes aligned along a single axis. Nonzeros exist in at least both x and y.");
128  }
129  else if( lengthVector.dot( btVector3(0, 0, 1)) != 0.0 ) {
130  // Box is not aligned along an axis, no prescription for such a case yet.
131  throw std::invalid_argument("tgBoxMoreAnchors is not aligned along an axis! This class currently only supports boxes aligned along a single axis. Nonzeros exist in both x and z.");
132  }
133  // Else, the vector only has only a nonzero in x.
134  boxOrientation = "x";
135  }
136  else if( lengthVector.dot( btVector3(0, 1, 0)) != 0.0) {
137  // There is a nonzero in y.
138  // Check if also a nonzero in z:
139  if( lengthVector.dot( btVector3(0, 0, 1)) != 0.0) {
140  // Box is not aligned along an axis, no prescription for such a case yet.
141  throw std::invalid_argument("tgBoxMoreAnchors is not aligned along an axis! This class currently only supports boxes aligned along a single axis. Nonzeros exist in both y and z.");
142  }
143  // Else, the vector only has a nonzero in y.
144  boxOrientation = "y";
145  }
146  else if( lengthVector.dot( btVector3(0, 0, 1)) != 0.0) {
147  // Box must be in z only.
148  boxOrientation = "z";
149  }
150  else {
151  // Must be zero length???
152  throw std::runtime_error("tgBoxMoreAnchors appears to have length zero. That's not possible. Error thrown inside tgBoxMoreAnchorsInfo::getBoxOrientation.");
153  }
154 
155  // Return the box's orientation.
156  return boxOrientation;
157 }
158 
170  // Length is in X, Width is in Y, Height is in Z.
171  // Used to be: width in z, height in y.
172  //DEBUGGING
173  //std::cout << "Length is in x direction, so width will be in y, "
174  // << "and height will be in z. " << std::endl;
175  return btVector3(getLength()/2, getConfig().width, getConfig().height);
176 }
177 btVector3 tgBoxMoreAnchorsInfo::getHalfExtentsOrientedY() const {
178  // Length is in Y, Width is in X, Height is in Z.
179  //DEBUGGING
180  //std::cout << "Length is in y direction, so width will be in x, "
181  // << "and height will be in z. " << std::endl;
182  return btVector3(getConfig().width, getLength()/2, getConfig().height);
183 }
184 btVector3 tgBoxMoreAnchorsInfo::getHalfExtentsOrientedZ() const {
185  // Length is in Z, Width is in X, Height is in Y.
186  // Used to be: width in Y, height in X.
187  //DEBUGGING
188  //std::cout << "Length is in z direction, so width will be in X, "
189  // << "and height will be in Y. " << std::endl;
190  return btVector3(getConfig().width, getConfig().height, getLength()/2);
191 }
192 
201  // The "half extents" of the box are the x, y, and z dimensions.
202  btVector3 halfExtents;
203 
204  // HOWEVER, we need to figure out which dimension corresponds to the length,
205  // width, and height of the box.
206  // This does not seem to be consistent between boxes created in different
207  // directions.
208  // So, check the orientation of the box, and then get the half extents for
209  // each case.
210  std::string orientation = getBoxOrientation();
211 
212  // Then, call the appropriate helper function to get the half extents,
213  // depending on the orientation.
214  if( orientation == "x" ) {
215  //DEBUGGING
216  //std::cout << "Orientation is x, calling getHalfExtentsOrientedX." << std::endl;
217  halfExtents = getHalfExtentsOrientedX();
218  }
219  else if( orientation == "y" ) {
220  //DEBUGGING
221  //std::cout << "Orientation is y, calling getHalfExtentsOrientedY." << std::endl;
222  halfExtents = getHalfExtentsOrientedY();
223  }
224  else if( orientation == "z" ) {
225  //DEBUGGING
226  //std::cout << "Orientation is z, calling getHalfExtentsOrientedZ." << std::endl;
227  halfExtents = getHalfExtentsOrientedZ();
228  }
229  else {
230  throw std::runtime_error("Inside tgBoxMoreAnchorsInfo::getHalfExtents, orientation is neither x nor y nor z! This class currently only works with boxes aligned along one axis.");
231  }
232 
233  return halfExtents;
234 }
235 
243 bool tgBoxMoreAnchorsInfo::isNodeOnBoxSurface(const btVector3& nodeVector) const {
244  // Like btBoxShape::isInside, first calculate the vector from the
245  // center of this box to the the node that's on the corner of the
246  // box where all coordinates are positive (the "top right" corner.)
247  // This vector starts from one node of the box, and goes halfway between
248  // the from and the to node (which should be the center of the box...)
249  btVector3 boxCenter = getFrom() + ( getTo() - getFrom() )/2;
250 
251  // Use the helper function to get the dimensions of the box:
252  btVector3 halfExtents = getHalfExtents();
253 
254  //DEBUGGING
255  if( 0 ){
256  std::cout << "Box has half extents: " << halfExtents << std::endl;
257  }
258 
259  //DEBUGGING
260  if( 0 ){
261  std::cout << "Checking tgBoxMoreAnchorsInfo::isNodeOnBoxSurface for box with "
262  << "pair " << getFrom() << ", " << getTo() << ", " << std::endl
263  << "...center of " << boxCenter << ", " << std::endl
264  << "...half-extents " << halfExtents << ", " << std::endl
265  << "...and node vector " << nodeVector << "." << std::endl;
266  std::cout << "NOTE that this computation uses fuzzy logic: a very small number"
267  << " is added or subtracted to the half extents. " << std::endl
268  << "Currently, this number is taken from Bullet Physics' SIMD_EPSILON"
269  << " and is equal to " << tgEpsilon << "." << std::endl;
270  }
271  // Point is on box surface if equal to either positive or negative extents
272  // in one dimension, and is within the +/- extents in the other two.
273  // This is checked by comparing the location of the point with respect to
274  // the box center plus or minus the half extents.
275  // For now, check inclusion on surface OR inside since that's easier.
276  // Version without fuzzy checking:
277  /*
278  bool result = (nodeVector.x() <= (boxCenter.x() + halfExtents.x())) &&
279  (nodeVector.x() >= (boxCenter.x() - halfExtents.x())) &&
280  (nodeVector.y() <= (boxCenter.y() + halfExtents.y())) &&
281  (nodeVector.y() >= (boxCenter.y() - halfExtents.y())) &&
282  (nodeVector.z() <= (boxCenter.z() + halfExtents.z())) &&
283  (nodeVector.z() >= (boxCenter.z() - halfExtents.z()));
284  */
285 
286  // Version WITH FUZZY CHECKING:
287  bool result = (nodeVector.x() <= (boxCenter.x() + halfExtents.x() + tgEpsilon)) &&
288  (nodeVector.x() >= (boxCenter.x() - halfExtents.x() - tgEpsilon)) &&
289  (nodeVector.y() <= (boxCenter.y() + halfExtents.y() + tgEpsilon)) &&
290  (nodeVector.y() >= (boxCenter.y() - halfExtents.y() - tgEpsilon)) &&
291  (nodeVector.z() <= (boxCenter.z() + halfExtents.z() + tgEpsilon)) &&
292  (nodeVector.z() >= (boxCenter.z() - halfExtents.z() - tgEpsilon));
293 
294  //DEBUGGING
295  //List the result of each individual check.
296  /*
297  std::cout << "List of bounding box checks in each direction: " << std::endl;
298  std::cout << "x_node <= x_box + halfExtents: "
299  << (nodeVector.x() <= (boxCenter.x() + halfExtents.x())) << std::endl
300  << "x_node >= x_box - halfExtents: "
301  << (nodeVector.x() >= (boxCenter.x() - halfExtents.x())) << std::endl
302  << "y_node <= y_box + halfExtents: "
303  << (nodeVector.y() <= (boxCenter.y() + halfExtents.y())) << std::endl
304  << "y_node >= y_box - halfExtents: "
305  << (nodeVector.y() >= (boxCenter.y() - halfExtents.y())) << std::endl
306  << "z_node <= z_box + halfExtents: "
307  << (nodeVector.z() <= (boxCenter.z() + halfExtents.z())) << std::endl
308  << "z_node >= z_box - halfExtents: "
309  << (nodeVector.z() >= (boxCenter.z() - halfExtents.z())) << std::endl;
310  */
311 
312  //DEBUGGING:
313  /*
314  std::cout << "Subtracting boxCenter.x() == " << boxCenter.x() << " minus "
315  << "halfExtents.x() == " << halfExtents.x() << " equals "
316  << boxCenter.x() - halfExtents.x() << ", compare to nodeVector.x() == "
317  << nodeVector.x() << "..." << std::endl;
318  */
319 
320  //DEBUGGING
321  if( 0 ) {
322  std::cout << "Result of node surface check: " << result << std::endl;
323  }
324  return result;
325 }
326 
331 bool tgBoxMoreAnchorsInfo::containsNode(const btVector3& nodeVector) const {
332  // DEBUGGING
333  if( 0 ){
334  std::cout << "Called containsNode inside tgBoxMoreAnchorsInfo with pair "
335  << getFrom() << ", " << getTo() << std::endl
336  << " and tags " << getTags()
337  << " for nodeVector "
338  << nodeVector << " ..." << std::endl;
339  }
340  // first do the older check:
341  if( ((getFrom() - nodeVector).fuzzyZero() || (getTo() - nodeVector).fuzzyZero()) ){
342  //DEBUGGING:
343  //std::cout << "containsNode: true, is part of pair." << std::endl << std::endl;
344  return true;
345  }
346  else {
347  // Check if it's inside or on the surface of the box.
348  //DEBUGGING:
349  //std::cout << "Node not either of the coordinates in this box's pair. "
350  // << "Now checking inside or on surface of box... " << std::endl;
351  if( isNodeOnBoxSurface(nodeVector) ) {
352  //DEBUGGING
353  //std::cout << "Node is on box surface, true! " << std::endl << std::endl;
354  return true;
355  }
356 
357  //DEBUGGING
358  //std::cout << "containsNode: false." << std::endl << std::endl;
359  return false;
360  }
361 }
362 
370 /*
371 std::set<btVector3> tgBoxInfo::getContainedNodes() const {
372  std::set<btVector3> contained;
373  contained.insert(getFrom());
374  contained.insert(getTo());
375  return contained;
376 }
377 */
Contains the definition of class tgWorldBulletPhysicsImpl.
virtual void initRigidBody(tgWorld &world)
Definition: tgBoxInfo.cpp:73
tgBoxMoreAnchorsInfo(const tgBox::Config &config)
const btVector3 & getFrom() const
Definition: tgBoxInfo.h:110
tgRigidInfo * createRigidInfo(const tgPair &pair)
const tgBox::Config & getConfig() const
Definition: tgBoxInfo.h:107
btVector3 getHalfExtents() const
const btVector3 & getTo() const
Definition: tgBoxInfo.h:113
std::string getBoxOrientation() const
btVector3 getHalfExtentsOrientedX() const
virtual btRigidBody * getRigidBody()
virtual tgModel * createModel(tgWorld &world)
Definition: tgPair.h:48
bool isNodeOnBoxSurface(const btVector3 &nodeVector) const
double getLength() const
Definition: tgBoxInfo.h:213
virtual std::set< tgRigidInfo * > getLeafRigids()
Class that interfaces with Bullet to build the boxes, specifically for boxes that support more than 2...
Definition: tgTags.h:44
virtual bool containsNode(const btVector3 &nodeVector) const