ContactTrackerSubsystem.h

Go to the documentation of this file.

#ifndef SimTK_SIMBODY_CONTACT_TRACKER_SUBSYSTEM_H_
#define SimTK_SIMBODY_CONTACT_TRACKER_SUBSYSTEM_H_

/* -------------------------------------------------------------------------- *
 *                               Simbody(tm)                                  *
 * -------------------------------------------------------------------------- *
 * This is part of the SimTK biosimulation toolkit originating from           *
 * Simbios, the NIH National Center for Physics-Based Simulation of           *
 * Biological Structures at Stanford, funded under the NIH Roadmap for        *
 * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody.  *
 *                                                                            *
 * Portions copyright (c) 2011-15 Stanford University and the Authors.        *
 * Authors: Michael Sherman, Peter Eastman                                    *
 * Contributors:                                                              *
 *                                                                            *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may    *
 * not use this file except in compliance with the License. You may obtain a  *
 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0.         *
 *                                                                            *
 * Unless required by applicable law or agreed to in writing, software        *
 * distributed under the License is distributed on an "AS IS" BASIS,          *
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *
 * See the License for the specific language governing permissions and        *
 * limitations under the License.                                             *
 * -------------------------------------------------------------------------- */

#include "SimTKmath.h"
#include "simbody/internal/common.h"
#include "simbody/internal/ContactSurface.h"

namespace SimTK {


class MultibodySystem;
class MobilizedBody;
class ContactTracker;
class Contact;
class ContactSnapshot;

//==============================================================================
//                          CONTACT TRACKER SUBSYSTEM
//==============================================================================
class SimTK_SIMBODY_EXPORT ContactTrackerSubsystem : public Subsystem {
public:
explicit ContactTrackerSubsystem(MultibodySystem& system);

const ContactSnapshot& getActiveContacts(const State& state) const;

const ContactSnapshot& getPredictedContacts(const State& state) const;
int getNumSurfaces() const;

ContactSurfaceIndex getContactSurfaceIndex(MobilizedBodyIndex mobod, 
                                           int contactSurfaceOrdinal) const;

const MobilizedBody& getMobilizedBody(ContactSurfaceIndex surfIx) const;

const ContactSurface& getContactSurface(ContactSurfaceIndex surfIx) const;

const Transform& getContactSurfaceTransform(ContactSurfaceIndex surfIx) const;
void adoptContactTracker(ContactTracker* tracker);

bool hasContactTracker(ContactGeometryTypeId surface1, 
                       ContactGeometryTypeId surface2) const;

const ContactTracker& getContactTracker(ContactGeometryTypeId surface1, 
                                        ContactGeometryTypeId surface2,
                                        bool& reverseOrder) const;
const ContactSnapshot& getPreviousActiveContacts(const State& state) const;

const ContactSnapshot& getPreviousPredictedContacts(const State& state) const;

bool realizeActiveContacts(const State& state, 
                           bool         lastTry,
                           Real&        stepAdvice) const;

bool realizePredictedContacts(const State& state, 
                              bool         lastTry,
                              Real&        stepAdvice) const;

ContactTrackerSubsystem();
SimTK_PIMPL_DOWNCAST(ContactTrackerSubsystem, Subsystem);

//--------------------------------------------------------------------------
private:
class ContactTrackerSubsystemImpl& updImpl();
const ContactTrackerSubsystemImpl& getImpl() const;
};



//==============================================================================
//                            CONTACT SNAPSHOT
//==============================================================================
class SimTK_SIMBODY_EXPORT ContactSnapshot {
//TODO: replace with fast hash tables
typedef std::map<ContactId,int>     ContactMap;
// Note: we always order the key so that the first surface index is less than
// the second (they can't be equal!).
typedef std::map<std::pair<ContactSurfaceIndex,ContactSurfaceIndex>, 
                 ContactId>         SurfaceMap;
public:
ContactSnapshot() : m_time(NaN) {}

void clear() {
    m_time = NaN;
    m_contacts.clear();
    m_id2contact.clear();
    m_surfPair2id.clear();
}

void setTimestamp(Real time) {m_time=time;}
Real getTimestamp() const {return m_time;}

void adoptContact(Contact& contact) {
    const ContactId id = contact.getContactId();
    ContactSurfaceIndex surf1(contact.getSurface1());
    ContactSurfaceIndex surf2(contact.getSurface2());
    assert(id.isValid() && surf1.isValid() && surf2.isValid());

    // Surface pair must be ordered (low,high) for the map.
    if (surf1 > surf2) std::swap(surf1,surf2);

    assert(!hasContact(id));
    assert(!hasContact(surf1,surf2));

    const int indx = m_contacts.size();
    m_contacts.push_back(contact); // shallow copy
    m_id2contact[id] = indx;
    m_surfPair2id[std::make_pair(surf1,surf2)] = id;
}

bool hasContact(ContactId id) const 
{   return m_id2contact.find(id) != m_id2contact.end(); }
bool hasContact(ContactSurfaceIndex surf1, ContactSurfaceIndex surf2) const
{   if (surf1 > surf2) std::swap(surf1,surf2);
    return m_surfPair2id.find(std::make_pair(surf1,surf2)) 
        != m_surfPair2id.end(); }

int getNumContacts() const {return m_contacts.size();}
const Contact& getContact(int n) const {return m_contacts[n];}
const Contact& getContactById(ContactId id) const
{   static Contact empty;
    ContactMap::const_iterator p = m_id2contact.find(id);
    return p==m_id2contact.end() ? empty : m_contacts[p->second]; }
ContactId getContactIdForSurfacePair(ContactSurfaceIndex surf1,
                                     ContactSurfaceIndex surf2) const
{   if (surf1 > surf2) std::swap(surf1,surf2);
    SurfaceMap::const_iterator p = 
        m_surfPair2id.find(std::make_pair(surf1,surf2));
    return p==m_surfPair2id.end() ? ContactId() : p->second; }

//--------------------------------------------------------------------------
                                private:

// Remove a Contact occupying a particular slot in the Contact array. This
// will result in another Contact object being moved to occupy the now-empty
// slot to keep the array compact.
void removeContact(int n) {
    assert(0 <= n && n < m_contacts.size());
    if (n+1 == m_contacts.size()) {
        m_contacts.pop_back(); // this was the last one
        return;
    }
    // Move the last one to replace this one and update the map.
    m_contacts[n] = m_contacts.back();  // shallow copy
    m_contacts.pop_back();              // destruct
    m_id2contact[m_contacts[n].getContactId()] = n;
}


Real                m_time;         // when this snapshot was taken
Array_<Contact,int> m_contacts;     // all the contact pairs
ContactMap          m_id2contact;   // the contact pairs by contactId
SurfaceMap          m_surfPair2id;  // surfacepair -> contactId map
};

// for debugging
inline std::ostream& operator<<(std::ostream& o, const ContactSnapshot& cs) {
    o << "Contact snapshot: time=" << cs.getTimestamp()
        << " numContacts=" << cs.getNumContacts() << std::endl;
    return o;
}


} // namespace SimTK

#endif // SimTK_SIMBODY_CONTACT_TRACKER_SUBSYSTEM_H_