Simbody  3.5
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-12 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 
144 //==============================================================================
145 // CONTACT TRACKER SUBSYSTEM
146 //==============================================================================
148 public:
151 
156 int getNumSurfaces() const;
158 const MobilizedBody& getMobilizedBody(ContactSurfaceIndex surfIx) const;
161 const ContactSurface& getContactSurface(ContactSurfaceIndex surfIx) const;
164 const Transform& getContactSurfaceTransform(ContactSurfaceIndex surfIx) const;
165 
171 void adoptContactTracker(ContactTracker* tracker);
172 
176 bool hasContactTracker(ContactGeometryTypeId surface1,
177  ContactGeometryTypeId surface2) const;
178 
184 const ContactTracker& getContactTracker(ContactGeometryTypeId surface1,
185  ContactGeometryTypeId surface2,
186  bool& reverseOrder) const;
187 
190 const ContactSnapshot& getPreviousActiveContacts(const State& state) const;
191 
194 const ContactSnapshot& getPreviousPredictedContacts(const State& state) const;
195 
207 const ContactSnapshot& getActiveContacts(const State& state) const;
208 
217 const ContactSnapshot& getPredictedContacts(const State& state) const;
218 
225 bool realizeActiveContacts(const State& state,
226  bool lastTry,
227  Real& stepAdvice) const;
228 
232 bool realizePredictedContacts(const State& state,
233  bool lastTry,
234  Real& stepAdvice) const;
235 
237 
238 //--------------------------------------------------------------------------
239 private:
240 class ContactTrackerSubsystemImpl& updImpl();
241 const ContactTrackerSubsystemImpl& getImpl() const;
242 };
243 
244 
245 
246 //==============================================================================
247 // CONTACT SNAPSHOT
248 //==============================================================================
260 //TODO: replace with fast hash tables
261 typedef std::map<ContactId,int> ContactMap;
262 // Note: we always order the key so that the first surface index is less than
263 // the second (they can't be equal!).
264 typedef std::map<std::pair<ContactSurfaceIndex,ContactSurfaceIndex>,
265  ContactId> SurfaceMap;
266 public:
268 ContactSnapshot() : m_time(NaN) {}
269 
271 void clear() {
272  m_time = NaN;
273  m_contacts.clear();
274  m_id2contact.clear();
275  m_surfPair2id.clear();
276 }
277 
279 void setTimestamp(Real time) {m_time=time;}
281 Real getTimestamp() const {return m_time;}
282 
288 void adoptContact(Contact& contact) {
289  const ContactId id = contact.getContactId();
290  ContactSurfaceIndex surf1(contact.getSurface1());
291  ContactSurfaceIndex surf2(contact.getSurface2());
292  assert(id.isValid() && surf1.isValid() && surf2.isValid());
293 
294  // Surface pair must be ordered (low,high) for the map.
295  if (surf1 > surf2) std::swap(surf1,surf2);
296 
297  assert(!hasContact(id));
298  assert(!hasContact(surf1,surf2));
299 
300  const int indx = m_contacts.size();
301  m_contacts.push_back(contact); // shallow copy
302  m_id2contact[id] = indx;
303  m_surfPair2id[std::make_pair(surf1,surf2)] = id;
304 }
305 
307 bool hasContact(ContactId id) const
308 { return m_id2contact.find(id) != m_id2contact.end(); }
312 { if (surf1 > surf2) std::swap(surf1,surf2);
313  return m_surfPair2id.find(std::make_pair(surf1,surf2))
314  != m_surfPair2id.end(); }
315 
317 int getNumContacts() const {return m_contacts.size();}
322 const Contact& getContact(int n) const {return m_contacts[n];}
327 { static Contact empty;
328  ContactMap::const_iterator p = m_id2contact.find(id);
329  return p==m_id2contact.end() ? empty : m_contacts[p->second]; }
334  ContactSurfaceIndex surf2) const
335 { if (surf1 > surf2) std::swap(surf1,surf2);
336  SurfaceMap::const_iterator p =
337  m_surfPair2id.find(std::make_pair(surf1,surf2));
338  return p==m_surfPair2id.end() ? ContactId() : p->second; }
339 
340 //--------------------------------------------------------------------------
341  private:
342 
343 // Remove a Contact occupying a particular slot in the Contact array. This
344 // will result in another Contact object being moved to occupy the now-empty
345 // slot to keep the array compact.
346 void removeContact(int n) {
347  assert(0 <= n && n < m_contacts.size());
348  if (n+1 == m_contacts.size()) {
349  m_contacts.pop_back(); // this was the last one
350  return;
351  }
352  // Move the last one to replace this one and update the map.
353  m_contacts[n] = m_contacts.back(); // shallow copy
354  m_contacts.pop_back(); // destruct
355  m_id2contact[m_contacts[n].getContactId()] = n;
356 }
357 
358 
359 Real m_time; // when this snapshot was taken
360 Array_<Contact,int> m_contacts; // all the contact pairs
361 ContactMap m_id2contact; // the contact pairs by contactId
362 SurfaceMap m_surfPair2id; // surfacepair -> contactId map
363 };
364 
365 // for debugging
366 inline std::ostream& operator<<(std::ostream& o, const ContactSnapshot& cs) {
367  o << "Contact snapshot: time=" << cs.getTimestamp()
368  << " numContacts=" << cs.getNumContacts() << std::endl;
369  return o;
370 }
371 
372 
373 } // namespace SimTK
374 
375 #endif // SimTK_SIMBODY_CONTACT_TRACKER_SUBSYSTEM_H_
A Subsystem is expected to be part of a larger System and to have interdependencies with other subsys...
Definition: Subsystem.h:55
const Contact & getContactById(ContactId id) const
If this snapshot contains a contact with the given id, return a reference to it; otherwise, return a reference to an empty contact handle (you can check with isEmpty()).
Definition: ContactTrackerSubsystem.h:326
This defines a unique index for all the contact surfaces being handled either by a ContactTrackerSubs...
ContactSurfaceIndex getSurface2() const
Get the second surface involved in the contact, specified by its index within its contact set or Cont...
Declares ContactMaterial and ContactSurface classes.
bool hasContact(ContactId id) const
Does this snapshot contain a Contact object with the given ContactId?
Definition: ContactTrackerSubsystem.h:307
This is a unique integer type for quickly identifying specific types of contact geometry for fast loo...
A ContactTracker implements an algorithm for detecting overlaps or potential overlaps between pairs o...
Definition: ContactTracker.h:62
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
int getNumContacts() const
Find out how many Contacts are in this snapshot.
Definition: ContactTrackerSubsystem.h:317
Every Simbody header and source file should include this header before any other Simbody header...
This is a unique integer Id assigned to each contact pair when we first begin to track it...
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:593
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:311
This object is intended to contain all state information for a SimTK::System, except topological info...
Definition: State.h:276
const Real NaN
This is the IEEE "not a number" constant for this implementation of the default-precision Real type; ...
const Contact & getContact(int n) const
Get a reference to the n&#39;th Contact in this snapshot; note that the position of a given Contact in th...
Definition: ContactTrackerSubsystem.h:322
ContactSnapshot()
Default constructor sets timestamp to NaN.
Definition: ContactTrackerSubsystem.h:268
The SimTK::Array_<T> container class is a plug-compatible replacement for the C++ standard template l...
Definition: Array.h:50
This subsystem identifies and tracks potential contacts between bodies of a multibody system...
Definition: ContactTrackerSubsystem.h:147
The job of the MultibodySystem class is to coordinate the activities of various subsystems which can ...
Definition: MultibodySystem.h:48
ContactId getContactIdForSurfacePair(ContactSurfaceIndex surf1, ContactSurfaceIndex surf2) const
If this snapshot contains a contact for the given pair of contact surfaces (order doesn&#39;t matter)...
Definition: ContactTrackerSubsystem.h:333
std::ostream & operator<<(std::ostream &o, const ContactForce &f)
Definition: CompliantContactSubsystem.h:387
ContactSurfaceIndex getSurface1() const
Get the first surface involved in the contact, specified by its index within its contact set or Conta...
ContactId getContactId() const
Get the persistent ContactId that has been assigned to this Contact object if there is one (otherwise...
Real getTimestamp() const
At what simulation time was this contact snapshot taken?
Definition: ContactTrackerSubsystem.h:281
void clear()
Restore to default-constructed condition.
Definition: ContactTrackerSubsystem.h:271
#define SimTK_SIMBODY_EXPORT
Definition: Simbody/include/simbody/internal/common.h:72
A MobilizedBody is Simbody&#39;s fundamental body-and-joint object used to parameterize a system&#39;s motion...
Definition: MobilizedBody.h:167
A Contact contains information about the spatial relationship between two surfaces that are near...
Definition: Contact.h:83
This class combines a piece of ContactGeometry with a ContactMaterial to make an object suitable for ...
Definition: ContactSurface.h:332
void setTimestamp(Real time)
Set the time at which this snapshot was taken.
Definition: ContactTrackerSubsystem.h:279
Objects of this class represent collections of surface-pair interactions that are being tracked at a ...
Definition: ContactTrackerSubsystem.h:259
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:288
#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:580