Simbody 3.7
Loading...
Searching...
No Matches
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
31namespace SimTK {
32
33
34class MultibodySystem;
35class MobilizedBody;
36class ContactTracker;
37class Contact;
38class ContactSnapshot;
39
145//==============================================================================
146// CONTACT TRACKER SUBSYSTEM
147//==============================================================================
149public:
153
168const ContactSnapshot& getActiveContacts(const State& state) const;
169
178const ContactSnapshot& getPredictedContacts(const State& state) const;
192int getNumSurfaces() const;
193
206 int contactSurfaceOrdinal) const;
207
210
214
231
236 ContactGeometryTypeId surface2) const;
237
244 ContactGeometryTypeId surface2,
245 bool& reverseOrder) const;
256
261
268bool realizeActiveContacts(const State& state,
269 bool lastTry,
270 Real& stepAdvice) const;
271
276 bool lastTry,
277 Real& stepAdvice) const;
278
285
286//--------------------------------------------------------------------------
287private:
288class ContactTrackerSubsystemImpl& updImpl();
289const ContactTrackerSubsystemImpl& getImpl() const;
290};
291
292
293
294//==============================================================================
295// CONTACT SNAPSHOT
296//==============================================================================
308//TODO: replace with fast hash tables
309typedef 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!).
312typedef std::map<std::pair<ContactSurfaceIndex,ContactSurfaceIndex>,
313 ContactId> SurfaceMap;
314public:
316ContactSnapshot() : m_time(NaN) {}
317
319void clear() {
320 m_time = NaN;
321 m_contacts.clear();
322 m_id2contact.clear();
323 m_surfPair2id.clear();
324}
325
327void setTimestamp(Real time) {m_time=time;}
329Real getTimestamp() const {return m_time;}
330
336void 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
355bool 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
365int getNumContacts() const {return m_contacts.size();}
370const 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.
394void 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
407Real m_time; // when this snapshot was taken
408Array_<Contact,int> m_contacts; // all the contact pairs
409ContactMap m_id2contact; // the contact pairs by contactId
410SurfaceMap m_surfPair2id; // surfacepair -> contactId map
411};
412
413// for debugging
414inline 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_
Declares ContactMaterial and ContactSurface classes.
Every Simbody header and source file should include this header before any other Simbody header.
#define SimTK_SIMBODY_EXPORT
Definition Simbody/include/simbody/internal/common.h:68
This is a unique integer type for quickly identifying specific types of contact geometry for fast loo...
This is a unique integer Id assigned to each contact pair when we first begin to track it.
Objects of this class represent collections of surface-pair interactions that are being tracked at a ...
Definition ContactTrackerSubsystem.h:307
void setTimestamp(Real time)
Set the time at which this snapshot was taken.
Definition ContactTrackerSubsystem.h:327
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
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
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
void clear()
Restore to default-constructed condition.
Definition ContactTrackerSubsystem.h:319
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
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
Real getTimestamp() const
At what simulation time was this contact snapshot taken?
Definition ContactTrackerSubsystem.h:329
int getNumContacts() const
Find out how many Contacts are in this snapshot.
Definition ContactTrackerSubsystem.h:365
bool hasContact(ContactId id) const
Does this snapshot contain a Contact object with the given ContactId?
Definition ContactTrackerSubsystem.h:355
ContactSnapshot()
Default constructor sets timestamp to NaN.
Definition ContactTrackerSubsystem.h:316
This defines a unique index for all the contact surfaces being handled either by a ContactTrackerSubs...
This class combines a piece of ContactGeometry with a ContactMaterial to make an object suitable for ...
Definition ContactSurface.h:342
This subsystem identifies and tracks potential contacts between the mobilized bodies of a multibody s...
Definition ContactTrackerSubsystem.h:148
void adoptContactTracker(ContactTracker *tracker)
Register the contact tracking algorithm to use for a particular pair of ContactGeometry types,...
ContactTrackerSubsystem()
(Advanced) Default constructor creates an empty Subsystem not associated with any MultibodySystem; no...
bool hasContactTracker(ContactGeometryTypeId surface1, ContactGeometryTypeId surface2) const
Return true if this subsystem has a contact tracker registered that can deal with ineractions between...
const ContactSnapshot & getPreviousPredictedContacts(const State &state) const
(Advanced) Obtain the value of the ContactSnapshot state variable representing the most recently pred...
const ContactSnapshot & getPredictedContacts(const State &state) const
(Advanced) Get an additional set of predicted Contacts that can be anticipated from current velocity ...
const ContactSurface & getContactSurface(ContactSurfaceIndex surfIx) const
Get the ContactSurface object (detailed geometry and material properties) that is associated with the...
const ContactSnapshot & getPreviousActiveContacts(const State &state) const
(Advanced) Obtain the value of the ContactSnapshot state variable representing the most recently know...
ContactTrackerSubsystem(MultibodySystem &system)
Create a new ContactTrackerSubsystem and install it into the given MultibodySystem.
ContactSurfaceIndex getContactSurfaceIndex(MobilizedBodyIndex mobod, int contactSurfaceOrdinal) const
Obtain the ContactSurfaceIndex that was assigned by this ContactTrackerSubsystem to a particular inst...
const MobilizedBody & getMobilizedBody(ContactSurfaceIndex surfIx) const
Get the MobilizedBody associated with a particular contact surface.
SimTK_PIMPL_DOWNCAST(ContactTrackerSubsystem, Subsystem)
const ContactTracker & getContactTracker(ContactGeometryTypeId surface1, ContactGeometryTypeId surface2, bool &reverseOrder) const
Return the contact tracker to be used for an interaction between the indicated types of contact geome...
const Transform & getContactSurfaceTransform(ContactSurfaceIndex surfIx) const
Get the transform X_BS that gives the pose of the indicated contact surface with respect to the body ...
bool realizePredictedContacts(const State &state, bool lastTry, Real &stepAdvice) const
(Advanced) Calculate the set of anticipated Contacts set at Acceleration stage if not already calcula...
bool realizeActiveContacts(const State &state, bool lastTry, Real &stepAdvice) const
(Advanced) Calculate the current ActiveContacts set at Position stage or later if it hasn't already b...
const ContactSnapshot & getActiveContacts(const State &state) const
Get the calculated value of the ContactSnapshot cache entry representing the current set of Contact p...
int getNumSurfaces() const
Get the number of surfaces being managed by this contact tracker subsystem.
A ContactTracker implements an algorithm for detecting overlaps or potential overlaps between pairs o...
Definition ContactTracker.h:62
A Contact contains information about the spatial relationship between two surfaces that are near,...
Definition Contact.h:85
ContactSurfaceIndex getSurface1() const
Get the first surface involved in the contact, specified by its index within its contact set or Conta...
ContactSurfaceIndex getSurface2() const
Get the second surface involved in the contact, specified by its index within its contact set or Cont...
ContactId getContactId() const
Get the persistent ContactId that has been assigned to this Contact object if there is one (otherwise...
This is for arrays indexed by mobilized body number within a subsystem (typically the SimbodyMatterSu...
A MobilizedBody is Simbody's fundamental body-and-joint object used to parameterize a system's motion...
Definition MobilizedBody.h:169
The job of the MultibodySystem class is to coordinate the activities of various subsystems which can ...
Definition MultibodySystem.h:48
This object is intended to contain all state information for a SimTK::System, except topological info...
Definition State.h:280
A Subsystem is expected to be part of a larger System and to have interdependencies with other subsys...
Definition Subsystem.h:55
const Real NaN
This is the IEEE "not a number" constant for this implementation of the default-precision Real type; ...
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition Assembler.h:37
std::ostream & operator<<(std::ostream &o, const ContactForce &f)
Definition CompliantContactSubsystem.h:387
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