Simbody  3.6
ContactTrackerSubsystem.h
Go to the documentation of this file.
1 #ifndef SimTK_SIMBODY_CONTACT_TRACKER_SUBSYSTEM_H_
2 #define SimTK_SIMBODY_CONTACT_TRACKER_SUBSYSTEM_H_
3 
4 /* -------------------------------------------------------------------------- *
5  * Simbody(tm) *
6  * -------------------------------------------------------------------------- *
7  * This is part of the SimTK biosimulation toolkit originating from *
8  * Simbios, the NIH National Center for Physics-Based Simulation of *
9  * Biological Structures at Stanford, funded under the NIH Roadmap for *
10  * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody. *
11  * *
12  * Portions copyright (c) 2011-15 Stanford University and the Authors. *
13  * Authors: Michael Sherman, Peter Eastman *
14  * Contributors: *
15  * *
16  * Licensed under the Apache License, Version 2.0 (the "License"); you may *
17  * not use this file except in compliance with the License. You may obtain a *
18  * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. *
19  * *
20  * Unless required by applicable law or agreed to in writing, software *
21  * distributed under the License is distributed on an "AS IS" BASIS, *
22  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. *
23  * See the License for the specific language governing permissions and *
24  * limitations under the License. *
25  * -------------------------------------------------------------------------- */
26 
27 #include "SimTKmath.h"
30 
31 namespace SimTK {
32 
33 
34 class MultibodySystem;
35 class MobilizedBody;
36 class ContactTracker;
37 class Contact;
38 class ContactSnapshot;
39 
145 //==============================================================================
146 // CONTACT TRACKER SUBSYSTEM
147 //==============================================================================
149 public:
152 explicit ContactTrackerSubsystem(MultibodySystem& system);
153 
168 const ContactSnapshot& getActiveContacts(const State& state) const;
169 
178 const ContactSnapshot& getPredictedContacts(const State& state) const;
192 int getNumSurfaces() const;
193 
205 ContactSurfaceIndex getContactSurfaceIndex(MobilizedBodyIndex mobod,
206  int contactSurfaceOrdinal) const;
207 
209 const MobilizedBody& getMobilizedBody(ContactSurfaceIndex surfIx) const;
210 
213 const ContactSurface& getContactSurface(ContactSurfaceIndex surfIx) const;
214 
217 const Transform& getContactSurfaceTransform(ContactSurfaceIndex surfIx) const;
230 void adoptContactTracker(ContactTracker* tracker);
231 
235 bool hasContactTracker(ContactGeometryTypeId surface1,
236  ContactGeometryTypeId surface2) const;
237 
243 const ContactTracker& getContactTracker(ContactGeometryTypeId surface1,
244  ContactGeometryTypeId surface2,
245  bool& reverseOrder) const;
255 const ContactSnapshot& getPreviousActiveContacts(const State& state) const;
256 
260 const ContactSnapshot& getPreviousPredictedContacts(const State& state) const;
261 
268 bool realizeActiveContacts(const State& state,
269  bool lastTry,
270  Real& stepAdvice) const;
271 
275 bool realizePredictedContacts(const State& state,
276  bool lastTry,
277  Real& stepAdvice) const;
278 
285 
286 //--------------------------------------------------------------------------
287 private:
288 class ContactTrackerSubsystemImpl& updImpl();
289 const ContactTrackerSubsystemImpl& getImpl() const;
290 };
291 
292 
293 
294 //==============================================================================
295 // CONTACT SNAPSHOT
296 //==============================================================================
308 //TODO: replace with fast hash tables
309 typedef std::map<ContactId,int> ContactMap;
310 // Note: we always order the key so that the first surface index is less than
311 // the second (they can't be equal!).
312 typedef std::map<std::pair<ContactSurfaceIndex,ContactSurfaceIndex>,
313  ContactId> SurfaceMap;
314 public:
316 ContactSnapshot() : m_time(NaN) {}
317 
319 void clear() {
320  m_time = NaN;
321  m_contacts.clear();
322  m_id2contact.clear();
323  m_surfPair2id.clear();
324 }
325 
327 void setTimestamp(Real time) {m_time=time;}
329 Real getTimestamp() const {return m_time;}
330 
336 void adoptContact(Contact& contact) {
337  const ContactId id = contact.getContactId();
338  ContactSurfaceIndex surf1(contact.getSurface1());
339  ContactSurfaceIndex surf2(contact.getSurface2());
340  assert(id.isValid() && surf1.isValid() && surf2.isValid());
341 
342  // Surface pair must be ordered (low,high) for the map.
343  if (surf1 > surf2) std::swap(surf1,surf2);
344 
345  assert(!hasContact(id));
346  assert(!hasContact(surf1,surf2));
347 
348  const int indx = m_contacts.size();
349  m_contacts.push_back(contact); // shallow copy
350  m_id2contact[id] = indx;
351  m_surfPair2id[std::make_pair(surf1,surf2)] = id;
352 }
353 
355 bool hasContact(ContactId id) const
356 { return m_id2contact.find(id) != m_id2contact.end(); }
360 { if (surf1 > surf2) std::swap(surf1,surf2);
361  return m_surfPair2id.find(std::make_pair(surf1,surf2))
362  != m_surfPair2id.end(); }
363 
365 int getNumContacts() const {return m_contacts.size();}
370 const Contact& getContact(int n) const {return m_contacts[n];}
375 { static Contact empty;
376  ContactMap::const_iterator p = m_id2contact.find(id);
377  return p==m_id2contact.end() ? empty : m_contacts[p->second]; }
382  ContactSurfaceIndex surf2) const
383 { if (surf1 > surf2) std::swap(surf1,surf2);
384  SurfaceMap::const_iterator p =
385  m_surfPair2id.find(std::make_pair(surf1,surf2));
386  return p==m_surfPair2id.end() ? ContactId() : p->second; }
387 
388 //--------------------------------------------------------------------------
389  private:
390 
391 // Remove a Contact occupying a particular slot in the Contact array. This
392 // will result in another Contact object being moved to occupy the now-empty
393 // slot to keep the array compact.
394 void removeContact(int n) {
395  assert(0 <= n && n < m_contacts.size());
396  if (n+1 == m_contacts.size()) {
397  m_contacts.pop_back(); // this was the last one
398  return;
399  }
400  // Move the last one to replace this one and update the map.
401  m_contacts[n] = m_contacts.back(); // shallow copy
402  m_contacts.pop_back(); // destruct
403  m_id2contact[m_contacts[n].getContactId()] = n;
404 }
405 
406 
407 Real m_time; // when this snapshot was taken
408 Array_<Contact,int> m_contacts; // all the contact pairs
409 ContactMap m_id2contact; // the contact pairs by contactId
410 SurfaceMap m_surfPair2id; // surfacepair -> contactId map
411 };
412 
413 // for debugging
414 inline std::ostream& operator<<(std::ostream& o, const ContactSnapshot& cs) {
415  o << "Contact snapshot: time=" << cs.getTimestamp()
416  << " numContacts=" << cs.getNumContacts() << std::endl;
417  return o;
418 }
419 
420 
421 } // namespace SimTK
422 
423 #endif // SimTK_SIMBODY_CONTACT_TRACKER_SUBSYSTEM_H_
SimTK::ContactSnapshot::getContactById
const Contact & getContactById(ContactId id) const
If this snapshot contains a contact with the given id, return a reference to it; otherwise,...
Definition: ContactTrackerSubsystem.h:374
SimTK::ContactSnapshot
Objects of this class represent collections of surface-pair interactions that are being tracked at a ...
Definition: ContactTrackerSubsystem.h:307
SimTK::State
This object is intended to contain all state information for a SimTK::System, except topological info...
Definition: State.h:280
MobilizedBodyIndex
SimTK::ContactSnapshot::getContactIdForSurfacePair
ContactId getContactIdForSurfacePair(ContactSurfaceIndex surf1, ContactSurfaceIndex surf2) const
If this snapshot contains a contact for the given pair of contact surfaces (order doesn't matter),...
Definition: ContactTrackerSubsystem.h:381
ContactGeometryTypeId
SimTK::Contact::getContactId
ContactId getContactId() const
Get the persistent ContactId that has been assigned to this Contact object if there is one (otherwise...
SimTK::Subsystem
A Subsystem is expected to be part of a larger System and to have interdependencies with other subsys...
Definition: Subsystem.h:55
SimTKmath.h
ContactId
SimTK::ContactSnapshot::hasContact
bool hasContact(ContactSurfaceIndex surf1, ContactSurfaceIndex surf2) const
Does this snapshot contain a Contact object for the given surface pair (in either order)?
Definition: ContactTrackerSubsystem.h:359
SimTK::Contact
A Contact contains information about the spatial relationship between two surfaces that are near,...
Definition: Contact.h:85
SimTK
This is a System that represents the dynamics of a particle moving along a smooth surface.
Definition: Assembler.h:37
SimTK::ContactSurface
This class combines a piece of ContactGeometry with a ContactMaterial to make an object suitable for ...
Definition: ContactSurface.h:342
SimTK::NaN
const Real NaN
This is the IEEE "not a number" constant for this implementation of the default-precision Real type; ...
SimTK_PIMPL_DOWNCAST
#define SimTK_PIMPL_DOWNCAST(Derived, Parent)
Similar to the above but for private implementation abstract classes, that is, abstract class hierarc...
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:593
SimTK::ContactSnapshot::setTimestamp
void setTimestamp(Real time)
Set the time at which this snapshot was taken.
Definition: ContactTrackerSubsystem.h:327
SimTK::ContactSnapshot::getNumContacts
int getNumContacts() const
Find out how many Contacts are in this snapshot.
Definition: ContactTrackerSubsystem.h:365
SimTK::ContactSnapshot::getTimestamp
Real getTimestamp() const
At what simulation time was this contact snapshot taken?
Definition: ContactTrackerSubsystem.h:329
SimTK::ContactTracker
A ContactTracker implements an algorithm for detecting overlaps or potential overlaps between pairs o...
Definition: ContactTracker.h:62
SimTK::MultibodySystem
The job of the MultibodySystem class is to coordinate the activities of various subsystems which can ...
Definition: MultibodySystem.h:48
SimTK::ContactSnapshot::adoptContact
void adoptContact(Contact &contact)
Add this Contact object to this snapshot; this is a shallow, reference-counted copy so the Contact ob...
Definition: ContactTrackerSubsystem.h:336
SimTK::MobilizedBody
A MobilizedBody is Simbody's fundamental body-and-joint object used to parameterize a system's motion...
Definition: MobilizedBody.h:168
SimTK::ContactSnapshot::hasContact
bool hasContact(ContactId id) const
Does this snapshot contain a Contact object with the given ContactId?
Definition: ContactTrackerSubsystem.h:355
SimTK_SIMBODY_EXPORT
#define SimTK_SIMBODY_EXPORT
Definition: Simbody/include/simbody/internal/common.h:68
SimTK::Contact::getSurface2
ContactSurfaceIndex getSurface2() const
Get the second surface involved in the contact, specified by its index within its contact set or Cont...
SimTK::Contact::getSurface1
ContactSurfaceIndex getSurface1() const
Get the first surface involved in the contact, specified by its index within its contact set or Conta...
SimTK::Real
SimTK_Real Real
This is the default compiled-in floating point type for SimTK, either float or double.
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:606
SimTK::ContactSnapshot::ContactSnapshot
ContactSnapshot()
Default constructor sets timestamp to NaN.
Definition: ContactTrackerSubsystem.h:316
SimTK::ContactSnapshot::getContact
const Contact & getContact(int n) const
Get a reference to the n'th Contact in this snapshot; note that the position of a given Contact in th...
Definition: ContactTrackerSubsystem.h:370
ContactSurfaceIndex
SimTK::ContactSnapshot::clear
void clear()
Restore to default-constructed condition.
Definition: ContactTrackerSubsystem.h:319
SimTK::ContactTrackerSubsystem
This subsystem identifies and tracks potential contacts between the mobilized bodies of a multibody s...
Definition: ContactTrackerSubsystem.h:148
SimTK::operator<<
std::ostream & operator<<(std::ostream &o, const ContactForce &f)
Definition: CompliantContactSubsystem.h:387
SimTK::Transform_< Real >
ContactSurface.h
common.h