NTRT Simulator  v1.1
 All Classes Namespaces Files Functions Variables Typedefs Friends Pages
tgBulletSpringCableAnchor.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 
27 // The BulletPhysics library
28 #include "BulletDynamics/Dynamics/btRigidBody.h"
29 #include "BulletCollision/CollisionShapes/btCollisionShape.h"
30 #include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
31 
32 // The C++ Standard Library
33 #include <iostream>
34 #include <cassert>
35 #include <stdexcept>
36 
37 // Does the contact normal get updated using the body's rotation? (99% sure its yes)
38 #define USE_BASIS
39 // Do we update the contact based on the manifold? - Causes contacts to be missed, doesn't prevent angular energy from accumulating
40 // Contact update also appears to magnify force direction issues
41 #define SKIP_CONTACT_UPDATE
42 //#define VERBOSE
43 
45  btVector3 worldPos,
46  btVector3 cn,
47  bool perm,
48  bool slide,
49  btPersistentManifold* m) :
50  tgSpringCableAnchor(worldPos, cn, perm, slide),
51  attachedBody(body),
52  // Find relative position
53  // This should give relative position in a default orientation.
54  attachedRelativeOriginalPosition(attachedBody->getWorldTransform().inverse() *
55  worldPos),
56 #ifdef USE_BASIS
57  contactNormal(attachedBody->getWorldTransform().inverse().getBasis() * cn),
58 #else
59  contactNormal(cn),
60 #endif
61  manifold(m)
62 {
63  assert(body);
64 
65  // Ensure we're either not using a manifold, or we currently have the right manifold
66  assert(manifold == NULL || body == manifold->getBody0() || body == manifold->getBody1());
67 }
68 
70 {
71  // World will delete attached body, bullet owns the manifolds
72 
73 }
74 
75 // This returns current position relative to the rigidbody.
77 {
78  const btTransform tr = attachedBody->getWorldTransform();
79  const btVector3 worldPos = tr * attachedRelativeOriginalPosition;
80  return worldPos-this->attachedBody->getCenterOfMassPosition();
81 }
82 
84 {
85  const btTransform tr = attachedBody->getWorldTransform();
86  return tr * attachedRelativeOriginalPosition;
87 }
88 
90 {
91  bool ret = false;
92 
93  // Only sliding anchors should have their positions changed
94  if (sliding)
95  {
97  // Figure out which body to use
98  bool useB = true;
99  if (manifold->getBody0() != attachedBody)
100  {
101  useB = false;
102  }
103 
104  if(useB || manifold->getBody1() == attachedBody)
105  {
106  btScalar length = INFINITY;
107 
108  int n = manifold->getNumContacts();
109 
110  btVector3 contactPos = getWorldPosition();
111  btVector3 newNormal = contactNormal;
112  btScalar dist = 0.0;
113 
114  // Find closest contact point in this manifold
115  for (int p = 0; p < n; p++)
116  {
117  const btManifoldPoint& pt = manifold->getContactPoint(p);
118 
119  // Original position picked at beginning
120  btVector3 pos = useB ? pt.m_positionWorldOnA : pt.m_positionWorldOnB;
121 
122  btScalar contactDist = (pos - newPos).length();
123 
124  if (contactDist < length)
125  {
126  length = contactDist;
127  contactPos = pos;
128 
129  btScalar directionSign = useB ? btScalar(1.0) : btScalar(-1.0);
130 
131 #ifdef USE_BASIS
132  newNormal = attachedBody->getWorldTransform().inverse().getBasis() * pt.m_normalWorldOnB * directionSign;
133 #else
134  newNormal = pt.m_normalWorldOnB * directionSign;
135 #endif
136  dist = pt.getDistance();
137 
138 #ifdef VERBOSE
139  if (n >= 2)
140  {
141  std::cout << "Extra contacts!! " << p << " " << dist << std::endl;
142  }
143 #else
144  // Suppress compiler warning for unused variable
145  (void) dist;
146 #endif
147  }
148 
149  }
150 
151  // We've lost this contact for some reason, skip update and delete
152  //if (!(dist > 0.0 && length < 0.01))
153  {
154  // If contact is sufficiently close, update
155  if (length < 0.1)
156  {
157  // This makes contact handling better in some cases and worse in other
158  // Better conservation of momentum without it, but contacts tend to exist a little too long
159  // Just deleting at this stage is better for sliding, but worse for contact with multiple bodies
160  attachedRelativeOriginalPosition = attachedBody->getWorldTransform().inverse() *
161  newPos;
162 
163  if ((newNormal + contactNormal).length() < 0.5)
164  {
165  #ifdef VERBOSE
166  std::cout<< "Reversed normal" << std::endl;
167  #endif
168  }
169  else
170  {
171  ret = true;
172  }
173 
174  // Update again here in case we have the original manifold??
175  #ifndef SKIP_CONTACT_UPDATE
176  contactNormal = newNormal;
177  #endif
178  }
179  // Check if the update was bad based on the original position, if not delete
180  else if ( (getWorldPosition() - contactPos).length() <= 0.1)
181  {
182  ret = true;
183  }
184  }
185  }
186  else
187  {
188 #ifdef VERBOSE
189  std::cout << "Manifold out of date!" << std::endl;
190 #endif
191  }
192  // Else: neither body is attached, delete
193  }
194  else
195  {
196  std::cerr << "Tried to update a static anchor" << std::endl;
197  // This will return as a delete, make sure you check the anchor is not permanent
198  }
199 
200  return ret;
201 }
202 
204 {
205 
206 #ifdef USE_BASIS
207  const btTransform tr = attachedBody->getWorldTransform();
208  btVector3 newNormal = (tr.getBasis() * contactNormal);
209  newNormal = newNormal.length() > 0.0 ? newNormal.normalize() : btVector3(0.0, 0.0, 0.0);
210  //assert(newNormal.length() == 1.0);
211  return newNormal;
212 #else
213  return contactNormal;
214 #endif
215 
216 }
217 
218 bool tgBulletSpringCableAnchor::updateManifold(btPersistentManifold* m)
219 {
220  bool ret = false;
221  // Does the new manifold actually affect the attached body
222  if (m && (m->getBody0() == attachedBody || m->getBody1() == attachedBody ))
223  {
224  std::pair<btScalar, btVector3> manifoldValues = getManifoldDistance(m);
225  btScalar newDist = manifoldValues.first;
226  // If the original manifold is NULL, just use the new one
227  if (!manifold)
228  {
229  //manifold = m;
230 #ifdef VERBOSE
231  std::cout << "Old manifold was NULL" << std::endl;
232 #endif
233  ret = true;
234  }
235  // Use new manifold
236  else if (getManifoldDistance(manifold).first >= newDist)
237  {
238  //manifold = m;
239  ret = true;
240  }
241 
242  // If we updated, ensure the new contact normal is good
243  if(ret)
244  {
245  btVector3 newNormal = manifoldValues.second;
246  if ((newNormal + contactNormal).length() < 0.5)
247  {
248  #ifdef VERBOSE
249  std::cout <<"Reversed normal during anchor update" << std::endl;
250  #endif
251  ret = false;
252  }
253  else
254  {
255  manifold = m;
256  }
257  #ifndef SKIP_CONTACT_UPDATE
258  // Updating here appears to break conservation of momentum
259  //contactNormal = newNormal;
260  #endif
261  }
262  }
263 #ifdef VERBOSE
264  if (!ret)
265  {
266  std::cout << "Failed to update manifold!" << std::endl;
267  }
268 #endif
269 
270  return ret;
271 }
272 
273 std::pair<btScalar, btVector3> tgBulletSpringCableAnchor::getManifoldDistance(btPersistentManifold* m) const
274 {
275  bool useB = true;
276 
277  btScalar length = INFINITY;
278  btVector3 newNormal = contactNormal;
279 
280  if (!permanent)
281  {
282  if (m->getBody0() != attachedBody)
283  {
284  useB = false;
285  }
286  if(useB || m->getBody1() == attachedBody)
287  {
288 
289  int n = m->getNumContacts();
290 
291  btVector3 contactPos = getWorldPosition();
292  btScalar dist = 0.0;
293  for (int p = 0; p < n; p++)
294  {
295  const btManifoldPoint& pt = m->getContactPoint(p);
296 
297  // Original position picked at beginning
298  btVector3 pos = useB ? pt.m_positionWorldOnA : pt.m_positionWorldOnB;
299 
300  btScalar contactDist = (pos - getWorldPosition()).length();
301 
302  if (contactDist < length)
303  {
304  length = contactDist;
305  contactPos = pos;
306 
307  btScalar directionSign = useB ? btScalar(1.0) : btScalar(-1.0);
308 
309  if (length < 0.1)
310  {
311  #ifdef USE_BASIS
312  newNormal = attachedBody->getWorldTransform().inverse().getBasis() * pt.m_normalWorldOnB * directionSign;
313  #else
314  newNormal = pt.m_normalWorldOnB * directionSign;
315  #endif
316  }
317 
318  dist = pt.getDistance();
319  #ifdef VERBOSE
320  if (n >= 2)
321  {
322  std::cout << "Extra contacts!! " << p << " length " << length << " dist: " << dist << std::endl;
323  }
324  #else
325  // Suppress compiler warning for unused variable
326  (void) dist;
327  #endif
328  }
329  }
330  }
331  }
332 
333  return std::make_pair<btScalar, btVector3> (length, newNormal);
334 }
tgBulletSpringCableAnchor(btRigidBody *body, btVector3 pos, btVector3 cn=btVector3(0.0, 0.0, 0.0), bool perm=true, bool slide=false, btPersistentManifold *m=NULL)
virtual btVector3 getWorldPosition() const
bool updateManifold(btPersistentManifold *m)
Definitions of class tgBulletSpringCableAnchor, formerly muscleAnchor.
std::pair< btScalar, btVector3 > getManifoldDistance(btPersistentManifold *m) const
virtual btVector3 getContactNormal() const
virtual btVector3 getRelativePosition() const
virtual bool setWorldPosition(btVector3 &newPos)