NTRT Simulator  v1.1
 All Classes Namespaces Files Functions Variables Typedefs Friends Pages
tgBulletContactSpringCable.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 object
29 
30 // NTRT
31 #include "tgcreator/tgUtil.h"
33 #include "core/tgCast.h"
34 #include "core/tgBulletUtil.h"
35 #include "core/tgWorld.h"
37 
38 // The Bullet Physics library
39 #include "btBulletDynamicsCommon.h"
40 #include "BulletCollision/CollisionDispatch/btGhostObject.h"
41 #include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h"
42 #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
43 #include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
44 #include "BulletCollision/BroadphaseCollision/btDispatcher.h"
45 #include "BulletDynamics/Dynamics/btDynamicsWorld.h"
46 #include "LinearMath/btDefaultMotionState.h"
47 #include "LinearMath/btQuaternion.h"
48 #include "LinearMath/btQuickprof.h"
49 
50 // The C++ Standard Library
51 #include <iostream>
52 #include <cmath> // abs
53 #include <stdexcept>
54 
55 //#define VERBOSE
56 
57 tgBulletContactSpringCable::tgBulletContactSpringCable(btPairCachingGhostObject* ghostObject,
58  tgWorld& world,
59  const std::vector<tgBulletSpringCableAnchor*>& anchors,
60  double coefK,
61  double dampingCoefficient,
62  double pretension,
63  double thickness,
64  double resolution) :
65 tgBulletSpringCable (anchors, coefK, dampingCoefficient, pretension),
66 m_ghostObject(ghostObject),
67 m_world(world),
68 m_thickness(thickness),
69 m_resolution(resolution)
70 {
71 
72 }
73 
75 {
76  btDynamicsWorld& m_dynamicsWorld = tgBulletUtil::worldToDynamicsWorld(m_world);
77  m_dynamicsWorld.removeCollisionObject(m_ghostObject);
78 
79  btCollisionShape* shape = m_ghostObject->getCollisionShape();
80  deleteCollisionShape(shape);
81  delete m_ghostObject;
82 }
83 
85 {
86  btScalar length = 0;
87 
88  std::size_t n = m_anchors.size() - 1;
89  for (std::size_t i = 0; i < n; i++)
90  {
91  length += (m_anchors[i]->getWorldPosition() - m_anchors[i+1]->getWorldPosition()).length();
92  }
93 
94  return length;
95 }
96 
98 {
99  updateManifolds();
100 #if (0) // Typically causes contacts to be lost
101  int numPruned = 1;
102  while (numPruned > 0)
103  {
104  numPruned = updateAnchorPositions();
105  }
106 #endif
107  updateAnchorList();
108 
109  // Update positions and remove bad anchors
110  pruneAnchors();
111 
112 #ifdef VERBOSE
113  if (getActualLength() > m_prevLength + 0.2)
114  {
115 // throw std::runtime_error("Large length change!");
116  std::cout << "Previous length " << m_prevLength << " actual length " << getActualLength() << std::endl;
117  }
118 #endif
119 
120  calculateAndApplyForce(dt);
121 
122  // Do this last so the ghost object gets populated with collisions before it is deleted
123  updateCollisionObject();
124 
125  assert(invariant());
126 }
127 
128 void tgBulletContactSpringCable::calculateAndApplyForce(double dt)
129 {
130 #ifndef BT_NO_PROFILE
131  BT_PROFILE("calculateAndApplyForce");
132 #endif //BT_NO_PROFILE
133 
134  const double tension = getTension();
135  const double currLength = getActualLength();
136 
137  const double deltaStretch = currLength - m_prevLength;
138  m_velocity = deltaStretch / dt;
139 
141 
142  if (btFabs(tension) * 1.0 < btFabs(m_damping))
143  {
144  m_damping =
145  (m_damping > 0.0 ? tension * 1.0 : -tension * 1.0);
146  }
147 
148  const double magnitude = tension + m_damping;
149 
150  // Apply forces
151  std::size_t n = m_anchors.size();
152 
153  for (std::size_t i = 0; i < n; i++)
154  {
155  btVector3 force = btVector3 (0.0, 0.0, 0.0);
156  if (i == 0)
157  {
158  btVector3 direction = m_anchors[i + 1]->getWorldPosition() - m_anchors[i]->getWorldPosition();
159  force = direction.normalize() * magnitude;
160  }
161  // Will likely only be true for the last anchor
162  else if (m_anchors[i]->sliding == false)
163  {
164  btVector3 direction = m_anchors[i]->getWorldPosition() - m_anchors[i - 1]->getWorldPosition();
165  force = -direction.normalize() * magnitude;
166  }
167  else if (i < n - 1)
168  {
169  // Already normalized
170  btVector3 direction = m_anchors[i]->getContactNormal();
171 
172  // Get normal to string
173  btVector3 back = m_anchors[i - 1]->getWorldPosition();
174  btVector3 current = m_anchors[i]->getWorldPosition();
175  btVector3 forward = m_anchors[i + 1]->getWorldPosition();
176 
177 
178  btVector3 first = (forward - current);
179  btVector3 second = (back - current);
180 
181  btVector3 forceDir = first.normalize() + second.normalize();
182 
183  // For sliding anchors, just figuring out directions for now
184  force = magnitude * forceDir;
185 
186  }
187  else
188  {
189  throw std::runtime_error("tgBulletContactSpringCable: First or last anchor is a sliding constraint!!");
190  }
191 
192  m_anchors[i]->force = force;
193 
194  }
195 
196  btVector3 totalForce(0.0, 0.0, 0.0);
197 
198  for (std::size_t i = 0; i < n; i++)
199  {
200  btRigidBody* body = m_anchors[i]->attachedBody;
201 
202  btVector3 contactPoint = m_anchors[i]->getRelativePosition();
203  body->activate();
204 
205  totalForce += m_anchors[i]->force;
206 
207  btVector3 impulse = m_anchors[i]->force* dt;
208 
209  body->applyImpulse(impulse, contactPoint);
210  }
211 
212  if (!totalForce.fuzzyZero())
213  {
214  std::cout << "Total Force Error! " << totalForce << std::endl;
215  throw std::runtime_error("Total force did not sum to zero!");
216  }
217 
218  // Finished calculating, so can store things
219  m_prevLength = currLength;
220 
221 }
222 
223 void tgBulletContactSpringCable::updateManifolds()
224 {
225 #ifndef BT_NO_PROFILE
226  BT_PROFILE("updateManifolds");
227 #endif //BT_NO_PROFILE
228 
229  // Copy this vector so we can remove as necessary
230 
231  btManifoldArray m_manifoldArray;
232  btVector3 m_touchingNormal;
233 
234  btBroadphaseInterface* const m_overlappingPairCache = tgBulletUtil::worldToDynamicsWorld(m_world).getBroadphase();
235 
236  // Only caches the pairs, they don't have a lot of useful information
237  btBroadphasePairArray& pairArray = m_ghostObject->getOverlappingPairCache()->getOverlappingPairArray();
238  int numPairs = pairArray.size();
239 
240  std::vector<tgBulletSpringCableAnchor*> rejectedAnchors;
241 
242  for (int i = 0; i < numPairs; i++)
243  {
244  m_manifoldArray.clear();
245 
246  const btBroadphasePair& pair = pairArray[i];
247 
248  // The real broadphase's pair cache has the useful info
249  btBroadphasePair* collisionPair = m_overlappingPairCache->getOverlappingPairCache()->findPair(pair.m_pProxy0,pair.m_pProxy1);
250 
251  btCollisionObject* obj0 = static_cast<btCollisionObject*>(collisionPair->m_pProxy0->m_clientObject);
252  btCollisionObject* obj1 = static_cast<btCollisionObject*>(collisionPair->m_pProxy1->m_clientObject);
253 
254  if (collisionPair->m_algorithm)
255  collisionPair->m_algorithm->getAllContactManifolds(m_manifoldArray);
256 
257  for (int j = 0; j < m_manifoldArray.size(); j++)
258  {
259  btPersistentManifold* manifold = m_manifoldArray[j];
260  btScalar directionSign = manifold->getBody0() == m_ghostObject ? btScalar(-1.0) : btScalar(1.0);
261 
262  for (int p=0; p < manifold->getNumContacts(); p++)
263  {
264  const btManifoldPoint& pt = manifold->getContactPoint(p);
265 
266  btScalar dist = pt.getDistance();
267 
268  if (dist < 0.0)
269  {
270 
271  m_touchingNormal = pt.m_normalWorldOnB * directionSign;
272 
273  btVector3 pos = directionSign < 0 ? pt.m_positionWorldOnB : pt.m_positionWorldOnA;
274 
275  btRigidBody* rb = NULL;
276 
277  //std::cout << pos << " " << manifold << " " << manifold->m_index1a << std::endl;
278 
279  if (manifold->getBody0() == m_ghostObject)
280  {
281  if (manifold->getBody1() == obj1)
282  rb = btRigidBody::upcast(obj1);
283  else if (manifold->getBody1() == obj0)
284  rb = btRigidBody::upcast(obj0);
285  else
286  {
287  throw std::runtime_error("Can't find the right object!!");
288  }
289  }
290  else
291  {
292  if (manifold->getBody0() == obj0)
293  rb = btRigidBody::upcast(obj0);
294  else if (manifold->getBody0() == obj1)
295  rb = btRigidBody::upcast(obj1);
296  else
297  {
298  throw std::runtime_error("Can't find the right object!!");
299  }
300  }
301 
302  if(rb)
303  {
304 
305  int anchorPos = findNearestPastAnchor(pos);
306  assert(anchorPos < (int)(m_anchors.size() - 1));
307 
308  // -1 means findNearestPastAnchor failed
309  if (anchorPos >= 0)
310  {
311  // Not permanent, sliding contact
312  tgBulletSpringCableAnchor* const newAnchor = new tgBulletSpringCableAnchor(rb, pos, m_touchingNormal, false, true, manifold);
313 
314 
315  tgBulletSpringCableAnchor* backAnchor = m_anchors[anchorPos];
316  tgBulletSpringCableAnchor* forwardAnchor = m_anchors[anchorPos + 1];
317 
318  btVector3 pos0 = backAnchor->getWorldPosition();
319  btVector3 pos2 = forwardAnchor->getWorldPosition();
320 
321  btVector3 lineA = (pos2 - pos);
322  btVector3 lineB = (pos0 - pos);
323 
324  btScalar lengthA = lineA.length();
325  btScalar lengthB = lineB.length();
326 
327  btScalar mDistB = backAnchor->getManifoldDistance(newAnchor->getManifold()).first;
328  btScalar mDistA = forwardAnchor->getManifoldDistance(newAnchor->getManifold()).first;
329 
330  //std::cout << "Update Manifolds " << newAnchor->getManifold() << std::endl;
331 
332  bool del = false;
333 
334  if (lengthB <= m_resolution && rb == backAnchor->attachedBody && mDistB < mDistA)
335  {
336  if(backAnchor->updateManifold(manifold))
337  del = true;
338  //std::cout << "UpdateB " << mDistB << std::endl;
339  }
340  if (lengthA <= m_resolution && rb == forwardAnchor->attachedBody && (!del || mDistA < mDistB))
341  {
342  if (forwardAnchor->updateManifold(manifold))
343  del = true;
344  //std::cout << "UpdateA " << mDistA << std::endl;
345  }
346 
347  if (del)
348  {
350  delete newAnchor;
351  }
352  else
353  {
354 
355  m_newAnchors.push_back(newAnchor);
356  } // If anchor passes distance tests
357  } // If we could find the anchor's position
358  } // If body is a rigid body
359  } // If distance less than 0.0
360  } // For number of contacts
361  } // For number of manifolds
362  } // For pairs of objects
363 
364 
365 }
366 
367 void tgBulletContactSpringCable::updateAnchorList()
368 {
369 #ifndef BT_NO_PROFILE
370  BT_PROFILE("updateAnchorList");
371 #endif //BT_NO_PROFILE
372  int numContacts = 2;
373 
374  btScalar startLength = getActualLength();
375 
376  while (m_newAnchors.size() > 0)
377  {
378  // Not permanent, sliding contact
379  tgBulletSpringCableAnchor* const newAnchor = m_newAnchors[0];
380  m_newAnchors.erase(m_newAnchors.begin());
381 
382  btVector3 pos1 = newAnchor->getWorldPosition();
383 
384  int anchorPos = findNearestPastAnchor(pos1);
385 
386  assert(anchorPos < (int) (m_anchors.size() - 1));
387 
388  // -1 means findNearestPastAnchor failed
389  if (anchorPos >= 0)
390  {
391 
392  tgBulletSpringCableAnchor* backAnchor = m_anchors[anchorPos];
393  tgBulletSpringCableAnchor* forwardAnchor = m_anchors[anchorPos + 1];
394 
395  btVector3 pos0 = backAnchor->getWorldPosition();
396  btVector3 pos2 = forwardAnchor->getWorldPosition();
397 
398  btVector3 lineA = (pos2 - pos1);
399  btVector3 lineB = (pos0 - pos1);
400 
401  btScalar lengthA = lineA.length();
402  btScalar lengthB = lineB.length();
403 
404  btVector3 contactNormal = newAnchor->getContactNormal();
405 
406  btScalar normalValue1 = (lineA).dot(contactNormal);
407  btScalar normalValue2 = (lineB).dot(contactNormal);
408 
409  bool del = false;
410 
411  btScalar mDistB = backAnchor->getManifoldDistance(newAnchor->getManifold()).first;
412  btScalar mDistA = forwardAnchor->getManifoldDistance(newAnchor->getManifold()).first;
413 
414  //std::cout << "Update anchor list " << newAnchor->getManifold() << std::endl;
415 
416  // These may have changed, so check again
417  if (lengthB <= m_resolution && newAnchor->attachedBody == backAnchor->attachedBody && mDistB < mDistA)
418  {
419  if(backAnchor->updateManifold(newAnchor->getManifold()))
420  {
421  del = true;
422  //std::cout << "UpdateB " << mDistB << std::endl;
423  }
424  }
425  if (lengthA <= m_resolution && newAnchor->attachedBody == forwardAnchor->attachedBody && (!del || mDistA < mDistB))
426  {
427  if(forwardAnchor->updateManifold(newAnchor->getManifold()))
428  del = true;
429  //std::cout << "UpdateA " << mDistA << std::endl;
430  }
431 
432  btVector3 backNormal = backAnchor->getContactNormal();
433  btVector3 forwardNormal = forwardAnchor->getContactNormal();
434 
435  if (del)
436  {
437  delete newAnchor;
438  }
439  else if(normalValue1 < 0.0 || normalValue2 < 0.0)
440  {
441  delete newAnchor;
442  }
443  else if ((backNormal.dot(contactNormal) < 0.0 && newAnchor->attachedBody == backAnchor->attachedBody) ||
444  (forwardNormal.dot(contactNormal) < 0.0 && newAnchor->attachedBody == forwardAnchor->attachedBody))
445  {
446 #ifdef VERBOSE
447  std::cout << "Deleting based on contact normals! " << backNormal.dot(contactNormal);
448  std::cout << " " << forwardNormal.dot(contactNormal) << std::endl;
449 #endif
450  delete newAnchor;
451  }
452  else
453  {
454 
455  m_anchorIt = m_anchors.begin() + anchorPos + 1;
456 
457  m_anchorIt = m_anchors.insert(m_anchorIt, newAnchor);
458 
459 #if (1) // Keeps the energy down very well
460  if (getActualLength() > m_prevLength + 2.0 * m_resolution)
461  {
462 #ifdef VERBOSE
463  std::cout << "Deleting anchor on basis of length " << std::endl;
464 #endif
465  deleteAnchor(anchorPos + 1);
466  }
467  else
468  {
469  numContacts++;
470  }
471 #else
472  numContacts++;
473 #endif
474 
475 #ifdef VERBOSE
476  std::cout << "Prev: " << m_prevLength << " LengthDiff " << startLength << " " << getActualLength();
477  std::cout << " Anchors " << m_anchors.size() << std::endl;
478 #endif
479  }
480  }
481  else
482  {
483  delete newAnchor;
484  }
485  }
486 
487  //std::cout << "contacts " << numContacts << " unprunedAnchors " << m_anchors.size();
488 
489  //std::cout << " prunedAnchors " << m_anchors.size() << std::endl;
490 
491 }
492 
493 int tgBulletContactSpringCable::updateAnchorPositions()
494 {
495  int numPruned = 0;
496  std::size_t i = 1;
497 
498  bool keep = true; //m_anchors[i]->updateContactNormal();
499 
500  while (i < m_anchors.size() - 1)
501  {
502  btVector3 back = m_anchors[i - 1]->getWorldPosition();
503  btVector3 current = m_anchors[i]->getWorldPosition();
504  btVector3 forward = m_anchors[i + 1]->getWorldPosition();
505 
506  btVector3 lineA = (forward - current);
507  btVector3 lineB = (back - current);
508 
509  btVector3 contactNormal = m_anchors[i]->getContactNormal();
510 
511  if (!m_anchors[i]->permanent)
512  {
513 #if (0)
514  btVector3 tangentMove = ((contactNormal)(lineA + lineB));
515 
516  //btVector3 tangentMove = (lineB + lineA).dot(tangentDir) * tangentDir / 2.0;
517 #else
518  btVector3 tangentDir = ( (lineB - lineA).normalize().cross(contactNormal)).normalize();
519  //btScalar tangentDot = (lineB + lineA).dot(tangentDir);
520  btVector3 tangentMove = (lineB + lineA).dot(tangentDir) * tangentDir;
521 #endif
522  btVector3 newPos = current + tangentMove;
523  // Check if new position is on body
524 #if (1)
525  if (!keep || !m_anchors[i]->setWorldPosition(newPos))
526  {
527  deleteAnchor(i);
528  numPruned++;
529  }
530  else
531  {
532  i++;
533  }
534 #else
535  m_anchors[i]->setWorldPosition(newPos);
536  i++;
537 #endif
538  }
539  else
540  {
541  i++;
542  }
543  }
544 
545  return numPruned;
546 }
547 
548 void tgBulletContactSpringCable::pruneAnchors()
549 {
550  int numPruned = 0;
551  int passes = 0;
552  std::size_t i = 1;
553 
554  // Check all anchors for manifold validity
555 #if (0)
556  while (i < m_anchors.size() - 1)
557  {
558  btVector3 oldPos = m_anchors[i]->getWorldPosition();
559  if (!m_anchors[i]->setWorldPosition(oldPos))
560  {
561  deleteAnchor(i);
562  numPruned++;
563  }
564  else
565  {
566  i++;
567  }
568  }
569 #else
570  while (i < m_anchors.size() - 1)
571  {
572  btPersistentManifold* m = m_anchors[i]->getManifold();
573  if (m_anchors[i]->getManifoldDistance(m).first == INFINITY)
574  {
575  deleteAnchor(i);
576  numPruned++;
577  }
578  else
579  {
580  i++;
581  }
582  }
583 #endif
584 
585 #ifdef VERBOSE
586  std::cout << "Pruned off the bat " << numPruned << std::endl;
587 #endif
588  // Attempt to eliminate points that would cause the string to push
589  while (numPruned > 0 || passes <= 3)
590  {
591  #ifndef BT_NO_PROFILE
592  BT_PROFILE("pruneAnchors");
593  #endif //BT_NO_PROFILE
594  numPruned = 0;
595 
596  numPruned = updateAnchorPositions();
597 
598  if( numPruned == 0)
599  {
600 
601  i = 1;
602  while (i < m_anchors.size() - 1)
603  {
604 
605  if (!m_anchors[i]->permanent)
606  {
607  btScalar normalValue1;
608  btScalar normalValue2;
609 
610  // Get new values
611 
612  btVector3 back = m_anchors[i - 1]->getWorldPosition();
613  btVector3 current = m_anchors[i]->getWorldPosition();
614  btVector3 forward = m_anchors[i + 1]->getWorldPosition();
615 
616  btVector3 lineA = (forward - current);
617  btVector3 lineB = (back - current);
618 
619  btVector3 contactNormal = m_anchors[i]->getContactNormal();
620 
621 
622  if (lineA.length() < m_resolution / 2.0 || lineB.length() < m_resolution / 2.0)
623  {
624  // Arbitrary value that deletes the nodes
625  normalValue1 = -1.0;
626  normalValue2 = -1.0;
627  }
628  else
629  {
630  //lineA.normalize();
631  //lineB.normalize();
632  //std::cout << "Normals " << std::btFabs(line.dot( m_anchors[i]->contactNormal)) << std::endl;
633 
634  normalValue1 = (lineA).dot(contactNormal);
635  normalValue2 = (lineB).dot(contactNormal);
636  }
637  if ((normalValue1 < 0.0) || (normalValue2 < 0.0))
638  {
639  #ifdef VERBOSE
640  std::cout << "Erased normal: " << normalValue1 << " " << normalValue2 << " ";
641  #endif
642  if (deleteAnchor(i))
643  {
644  numPruned++;
645  }
646  else
647  {
648  // Permanent anchor, move on
649  i++;
650  }
651  }
652  else
653  {
654  i++;
655  }
656  }
657  }
658  }
659 
660  if (numPruned == 0)
661  {
662  passes++;
663  }
664  else
665  {
666  passes = 0;
667  }
668  }
669 
670  //std::cout << " Good Normal " << m_anchors.size();
671 
672 #ifdef VERBOSE
673  std::size_t n = m_anchors.size();
674  for (i = 0; i < n; i++)
675  {
676  std::cout << m_anchors[i]->getWorldPosition() << std::endl;
677  }
678 #endif
679 
680 }
681 
682 // This works ok at the moment. Need an effective way of determining if the rope is under an object
683 void tgBulletContactSpringCable::updateCollisionObject()
684 {
685 #ifndef BT_NO_PROFILE
686  BT_PROFILE("updateCollisionObject");
687 #endif //BT_NO_PROFILE
688 
689  btDispatcher* m_dispatcher = tgBulletUtil::worldToDynamicsWorld(m_world).getDispatcher();
690  btBroadphaseInterface* const m_overlappingPairCache = tgBulletUtil::worldToDynamicsWorld(m_world).getBroadphase();
691 
692  // Clear the existing child shapes
693  btCompoundShape* m_compoundShape = tgCast::cast<btCollisionShape, btCompoundShape> (m_ghostObject->getCollisionShape());
694  clearCompoundShape(m_compoundShape);
695 
696  btVector3 maxes(anchor2->getWorldPosition());
697  btVector3 mins(anchor1->getWorldPosition());
698 
699  std::size_t n = m_anchors.size();
700 
701  for (std::size_t i = 0; i < n; i++)
702  {
703  btVector3 worldPos = m_anchors[i]->getWorldPosition();
704  for (std::size_t j = 0; j < 3; j++)
705  {
706  if (worldPos[j] > maxes[j])
707  {
708  maxes[j] = worldPos[j];
709  }
710  if (worldPos[j] < mins[j])
711  {
712  mins[j] = worldPos[j];
713  }
714  }
715  }
716  btVector3 center = (maxes + mins)/2.0;
717 
718  btVector3 from = anchor1->getWorldPosition();
719  btVector3 to = anchor2->getWorldPosition();
720 
721  for (std::size_t i = 0; i < n-1; i++)
722  {
723  btVector3 pos1 = m_anchors[i]->getWorldPosition();
724  btVector3 pos2 = m_anchors[i+1]->getWorldPosition();
725 
726  // Children handles the orientation data
727  btTransform t = tgUtil::getTransform(pos2, pos1);
728  t.setOrigin(t.getOrigin() - center);
729 
730  btScalar length = (pos2 - pos1).length() / 2.0;
731 
733  btCylinderShape* box = new btCylinderShape(btVector3(m_thickness, length, m_thickness));
734 
735  m_compoundShape->addChildShape(t, box);
736  }
737  // Default margin is 0.04, so larger than default thickness. Behavior is better with larger margin
738  //m_compoundShape->setMargin(m_thickness);
739 
740  btTransform transform;
741  transform.setOrigin(center);
742  transform.setRotation(btQuaternion::getIdentity());
743 
744  m_ghostObject->setCollisionShape (m_compoundShape);
745  m_ghostObject->setWorldTransform(transform);
746 
747  // Delete the existing contacts in bullet to prevent sticking - may exacerbate problems with rotations
748  m_overlappingPairCache->getOverlappingPairCache()->cleanProxyFromPairs(m_ghostObject->getBroadphaseHandle(),m_dispatcher);
749 }
750 
751 void tgBulletContactSpringCable::deleteCollisionShape(btCollisionShape* pShape)
752 {
753 #ifndef BT_NO_PROFILE
754  BT_PROFILE("deleteCollisionShape");
755 #endif //BT_NO_PROFILE
756 
757  if (pShape)
758  {
759  btCompoundShape* cShape = tgCast::cast<btCollisionShape, btCompoundShape>(pShape);
760  if (cShape)
761  {
762  std::size_t n = cShape->getNumChildShapes();
763  for( std::size_t i = 0; i < n; i++)
764  {
765  deleteCollisionShape(cShape->getChildShape(i));
766  }
767  }
768 
769  delete pShape;
770  }
771 }
772 
773 void tgBulletContactSpringCable::clearCompoundShape(btCompoundShape* pShape)
774 {
775 #ifndef BT_NO_PROFILE
776  BT_PROFILE("clearCompoundShape");
777 #endif //BT_NO_PROFILE
778 
779  if (pShape)
780  {
781  while (pShape->getNumChildShapes() > 0)
782  {
783  btCollisionShape* pCShape = pShape->getChildShape(0);
784  deleteCollisionShape(pCShape);
785  pShape->removeChildShapeByIndex(0);
786  }
787  }
788 
789 }
790 
791 bool tgBulletContactSpringCable::deleteAnchor(int i)
792 {
793 #ifndef BT_NO_PROFILE
794  BT_PROFILE("deleteAnchor");
795 #endif //BT_NO_PROFILE
796  assert(i < m_anchors.size() && i >= 0);
797 
798  if (m_anchors[i]->permanent != true)
799  {
800  delete m_anchors[i];
801  m_anchors.erase(m_anchors.begin() + i);
802  return true;
803  }
804  else
805  {
806  return false;
807  }
808 }
809 
810 int tgBulletContactSpringCable::findNearestPastAnchor(btVector3& pos)
811 {
812 
813  std::size_t i = 0;
814  std::size_t n = m_anchors.size() - 1;
815  assert (n >= 1);
816 
819  // Start by determining the "correct" position
820  btScalar startDist = (pos - m_anchors[i]->getWorldPosition()).length();
821  btScalar dist = startDist;
822 
823  while (dist <= startDist && i < n)
824  {
825  i++;
826  btVector3 anchorPos = m_anchors[i]->getWorldPosition();
827  dist = (pos - anchorPos).length();
828  if (dist < startDist)
829  {
830  startDist = dist;
831  }
832  }
833 
834  // Check if we hit both break conditions at the same time
835  if (dist > startDist)
836  {
837  i--;
838  }
839 
840  if (i == n)
841  {
842  i--;
843  }
844 
845  if (i == 0)
846  {
847  // Do nothing
848  }
849  else if (n > 1)
850  {
851  // Know we've got 3 anchors, so we need to compare along the line
854 
855  tgBulletContactSpringCable::anchorCompare m_acTemp(a0, an);
856 
857  btVector3 current = m_anchors[i]->getWorldPosition();
858 
859  m_acTemp.comparePoints(pos, current) ? i-- : i+=0;
860 
861  assert((m_anchors[i]->getWorldPosition() - pos).length() <= (a0->getWorldPosition() - pos).length());
862  }
863 
864  // Check to make sure it's actually in this line
867 
868  btVector3 current = a0->getWorldPosition();
869  tgBulletContactSpringCable::anchorCompare m_acTemp(a0, an);
870  if( m_acTemp.comparePoints(current, pos))
871  {
872  // Success! do nothing, move on
873  }
874  else
875  {
876  //std::cout << "iterating backwards!" << std::endl;
877  // Start over, iterate from the back, see if its better
878  btScalar endDist = (pos - m_anchors[n]->getWorldPosition()).length();
879  btScalar dist2 = endDist;
880 
881  int j = n;
882 
883  while (dist2 <= endDist && j > 0)
884  {
885  j--;
886  btVector3 anchorPos = m_anchors[j]->getWorldPosition();
887  dist2 = (pos - anchorPos).length();
888  if (dist2 < endDist)
889  {
890  endDist = dist2;
891  }
892  }
893 
894  if (j == n)
895  {
896  j--;
897  }
898 
899  if (j == 0)
900  {
901  // Do nothing
902  }
903  else if (n > 1)
904  {
905  // Know we've got 3 anchors, so we need to compare along the line
908 
909  tgBulletContactSpringCable::anchorCompare m_acTemp(a0, an);
910 
911  btVector3 current = m_anchors[j]->getWorldPosition();
912 
913  m_acTemp.comparePoints(pos, current) ? j-- : j+=0;
914 
915  // This assert doesn't work due to iteration order. Is there a comparable assert?
916  //assert((m_anchors[j]->getWorldPosition() - pos).length() <= (a0->getWorldPosition() - pos).length());
917  }
918 
919  // Check to make sure it's actually in this line
922 
923  btVector3 current = a0->getWorldPosition();
924  tgBulletContactSpringCable::anchorCompare m_acTemp(a0, an);
925  if( m_acTemp.comparePoints(current, pos))
926  {
927  // Success! Set i to j and return
928  i = j;
929  }
930  else
931  {
932  if ( i !=j )
933  {
934  std::cout << "Error in iteration order First try: " << i << " Second Try: " << j << std::endl;
935  //throw std::runtime_error("Neither the front nor back iterations worked!");
936  }
937 
938  // Return failure
939  return -1;
940  }
941  }
942 
943  tgBulletSpringCableAnchor* prevAnchor = m_anchors[i];
944  assert (prevAnchor);
945 
946  return i;
947 
948 }
949 
950 tgBulletContactSpringCable::anchorCompare::anchorCompare(const tgBulletSpringCableAnchor* m1, const tgBulletSpringCableAnchor* m2) :
951 ma1(m1),
952 ma2(m2)
953 {
954 
955 }
956 
957 bool tgBulletContactSpringCable::anchorCompare::operator() (const tgBulletSpringCableAnchor* lhs, const tgBulletSpringCableAnchor* rhs) const
958 {
959  btVector3 pt2 = lhs->getWorldPosition();
960  btVector3 pt3 = rhs->getWorldPosition();
961 
962  return comparePoints(pt2, pt3);
963 }
964 
965 bool tgBulletContactSpringCable::anchorCompare::comparePoints(btVector3& pt2, btVector3& pt3) const
966 {
967  // @todo make sure these are good anchors. Assert?
968  btVector3 pt1 = ma1->getWorldPosition();
969  btVector3 ptN = ma2->getWorldPosition();
970 
971  btScalar lhDot = (ptN - pt1).dot(pt2);
972  btScalar rhDot = (ptN - pt1).dot(pt3);
973 
974  return lhDot < rhDot;
975 
976 }
977 
978 bool tgBulletContactSpringCable::invariant(void) const
979 {
980  return (m_coefK > 0.0 &&
981  m_dampingCoefficient >= 0.0 &&
982  m_prevLength >= 0.0 &&
983  m_restLength >= 0.0 &&
984  anchor1 != NULL &&
985  anchor2 != NULL &&
986  m_anchors.size() >= 2 &&
987  m_thickness >= 0.0 &&
988  m_resolution > 0 &&
989  m_ghostObject != NULL &&
990  m_newAnchors.size() == 0);
991 }
tgBulletContactSpringCable(btPairCachingGhostObject *ghostObject, tgWorld &world, const std::vector< tgBulletSpringCableAnchor * > &anchors, double coefK, double dampingCoefficient, double pretension=0.0, double thickness=0.001, double resolution=0.1)
Contains the definition of class tgWorldBulletPhysicsImpl.
static btDynamicsWorld & worldToDynamicsWorld(const tgWorld &world)
Utility class for class casting and filtering collections by type.
const double m_dampingCoefficient
btPersistentManifold * getManifold() const
virtual btVector3 getWorldPosition() const
tgBulletSpringCableAnchor *const anchor2
virtual const btScalar getActualLength() const
virtual const double getTension() const
std::vector< tgBulletSpringCableAnchor * > m_anchors
static btTransform getTransform(const btVector3 &startOrientation, const btVector3 &start, const btVector3 &end)
Definition: tgUtil.h:90
Contains the definition of class tgWorld $Id$.
Contains the definition of class tgBulletUtil.
bool updateManifold(btPersistentManifold *m)
double m_restLength
Definitions of class tgBulletSpringCableAnchor, formerly muscleAnchor.
std::pair< btScalar, btVector3 > getManifoldDistance(btPersistentManifold *m) const
btPairCachingGhostObject * m_ghostObject
Contains the definition of class tgUtil and overloaded operator<<() free functions.
virtual btVector3 getContactNormal() const
tgBulletSpringCableAnchor *const anchor1
double m_prevLength
Definition of a massless cable with contact dynamics.
const double m_coefK