ContactTrackerSubsystem.h

Go to the documentation of this file.

#ifndef SimTK_SIMBODY_CONTACT_TRACKER_SUBSYSTEM_H_
#define SimTK_SIMBODY_CONTACT_TRACKER_SUBSYSTEM_H_

/* -------------------------------------------------------------------------- *
 *                      SimTK Core: SimTK Simbody(tm)                         *
 * -------------------------------------------------------------------------- *
 * This is part of the SimTK Core 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.               *
 *                                                                            *
 * Portions copyright (c) 2010 Stanford University and the Authors.           *
 * Authors: Michael Sherman, Peter Eastman                                    *
 * Contributors:                                                              *
 *                                                                            *
 * Permission is hereby granted, free of charge, to any person obtaining a    *
 * copy of this software and associated documentation files (the "Software"), *
 * to deal in the Software without restriction, including without limitation  *
 * the rights to use, copy, modify, merge, publish, distribute, sublicense,   *
 * and/or sell copies of the Software, and to permit persons to whom the      *
 * Software is furnished to do so, subject to the following conditions:       *
 *                                                                            *
 * The above copyright notice and this permission notice shall be included in *
 * all copies or substantial portions of the Software.                        *
 *                                                                            *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,   *
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL    *
 * THE AUTHORS, CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,    *
 * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR      *
 * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE  *
 * USE OR OTHER DEALINGS IN THE SOFTWARE.                                     *
 * -------------------------------------------------------------------------- */

#include "SimTKcommon.h"
#include "simbody/internal/common.h"
#include "simbody/internal/ContactGeometry.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:
ContactTrackerSubsystem();
explicit ContactTrackerSubsystem(MultibodySystem&);

int getNumSurfaces() 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;

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

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

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

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

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;
}



//==============================================================================
//                              CONTACT TRACKER
//==============================================================================
class SimTK_SIMBODY_EXPORT ContactTracker {
public:
class HalfSpaceSphere;
class HalfSpaceEllipsoid;
class SphereSphere;
class HalfSpaceTriangleMesh;
class SphereTriangleMesh;
class TriangleMeshTriangleMesh;

ContactTracker(ContactGeometryTypeId typeOfSurface1,
               ContactGeometryTypeId typeOfSurface2)
:   m_surfaceTypes(typeOfSurface1, typeOfSurface2)
{
}

const std::pair<ContactGeometryTypeId,ContactGeometryTypeId>&
getContactGeometryTypeIds() const {return m_surfaceTypes;}

virtual ~ContactTracker() {}

virtual bool trackContact
   (const Contact&         priorStatus,
    const Transform& X_GS1, 
    const ContactGeometry& surface1,
    const Transform& X_GS2, 
    const ContactGeometry& surface2,
    Real                   cutoff,
    Contact&               currentStatus) const = 0;

virtual bool predictContact
   (const Contact&         priorStatus,
    const Transform& X_GS1, const SpatialVec& V_GS1, const SpatialVec& A_GS1,
    const ContactGeometry& surface1,
    const Transform& X_GS2, const SpatialVec& V_GS2, const SpatialVec& A_GS2,
    const ContactGeometry& surface2,
    Real                   cutoff,
    Real                   intervalOfInterest,
    Contact&               predictedStatus) const = 0;

virtual bool initializeContact
   (const Transform& X_GS1, const SpatialVec& V_GS1,
    const ContactGeometry& surface1,
    const Transform& X_GS2, const SpatialVec& V_GS2,
    const ContactGeometry& surface2,
    Real                   cutoff,
    Real                   intervalOfInterest,
    Contact&               contactStatus) const = 0;

//--------------------------------------------------------------------------
                                private:
// This tracker should be called only for surfaces of these two types,
// in this order.
std::pair<ContactGeometryTypeId,ContactGeometryTypeId> m_surfaceTypes;
};



//==============================================================================
//                     HALFSPACE-SPHERE CONTACT TRACKER
//==============================================================================
class SimTK_SIMBODY_EXPORT ContactTracker::HalfSpaceSphere 
:   public ContactTracker {
public:
HalfSpaceSphere() 
:   ContactTracker(ContactGeometry::HalfSpace::classTypeId(),
                   ContactGeometry::Sphere::classTypeId()) {}

virtual ~HalfSpaceSphere() {}

virtual bool trackContact
   (const Contact&         priorStatus,
    const Transform& X_GS1, 
    const ContactGeometry& surface1,
    const Transform& X_GS2, 
    const ContactGeometry& surface2,
    Real                   cutoff,
    Contact&               currentStatus) const;

virtual bool predictContact
   (const Contact&         priorStatus,
    const Transform& X_GS1, const SpatialVec& V_GS1, const SpatialVec& A_GS1,
    const ContactGeometry& surface1,
    const Transform& X_GS2, const SpatialVec& V_GS2, const SpatialVec& A_GS2,
    const ContactGeometry& surface2,
    Real                   cutoff,
    Real                   intervalOfInterest,
    Contact&               predictedStatus) const;

virtual bool initializeContact
   (const Transform& X_GS1, const SpatialVec& V_GS1,
    const ContactGeometry& surface1,
    const Transform& X_GS2, const SpatialVec& V_GS2,
    const ContactGeometry& surface2,
    Real                   cutoff,
    Real                   intervalOfInterest,
    Contact&               contactStatus) const;
};



//==============================================================================
//                     HALFSPACE-ELLIPSOID CONTACT TRACKER
//==============================================================================
class SimTK_SIMBODY_EXPORT ContactTracker::HalfSpaceEllipsoid 
:   public ContactTracker {
public:
HalfSpaceEllipsoid() 
:   ContactTracker(ContactGeometry::HalfSpace::classTypeId(),
                   ContactGeometry::Ellipsoid::classTypeId()) {}

virtual ~HalfSpaceEllipsoid() {}

virtual bool trackContact
   (const Contact&         priorStatus,
    const Transform& X_GS1, 
    const ContactGeometry& surface1,
    const Transform& X_GS2, 
    const ContactGeometry& surface2,
    Real                   cutoff,
    Contact&               currentStatus) const;

virtual bool predictContact
   (const Contact&         priorStatus,
    const Transform& X_GS1, const SpatialVec& V_GS1, const SpatialVec& A_GS1,
    const ContactGeometry& surface1,
    const Transform& X_GS2, const SpatialVec& V_GS2, const SpatialVec& A_GS2,
    const ContactGeometry& surface2,
    Real                   cutoff,
    Real                   intervalOfInterest,
    Contact&               predictedStatus) const;

virtual bool initializeContact
   (const Transform& X_GS1, const SpatialVec& V_GS1,
    const ContactGeometry& surface1,
    const Transform& X_GS2, const SpatialVec& V_GS2,
    const ContactGeometry& surface2,
    Real                   cutoff,
    Real                   intervalOfInterest,
    Contact&               contactStatus) const;
};



//==============================================================================
//                       SPHERE-SPHERE CONTACT TRACKER
//==============================================================================
class SimTK_SIMBODY_EXPORT ContactTracker::SphereSphere 
:   public ContactTracker {
public:
SphereSphere() 
:   ContactTracker(ContactGeometry::Sphere::classTypeId(),
                   ContactGeometry::Sphere::classTypeId()) {}

virtual ~SphereSphere() {}

virtual bool trackContact
   (const Contact&         priorStatus,
    const Transform& X_GS1, 
    const ContactGeometry& surface1,
    const Transform& X_GS2, 
    const ContactGeometry& surface2,
    Real                   cutoff,
    Contact&               currentStatus) const;

virtual bool predictContact
   (const Contact&         priorStatus,
    const Transform& X_GS1, const SpatialVec& V_GS1, const SpatialVec& A_GS1,
    const ContactGeometry& surface1,
    const Transform& X_GS2, const SpatialVec& V_GS2, const SpatialVec& A_GS2,
    const ContactGeometry& surface2,
    Real                   cutoff,
    Real                   intervalOfInterest,
    Contact&               predictedStatus) const;

virtual bool initializeContact
   (const Transform& X_GS1, const SpatialVec& V_GS1,
    const ContactGeometry& surface1,
    const Transform& X_GS2, const SpatialVec& V_GS2,
    const ContactGeometry& surface2,
    Real                   cutoff,
    Real                   intervalOfInterest,
    Contact&               contactStatus) const;
};



//==============================================================================
//                 HALFSPACE-TRIANGLE MESH CONTACT TRACKER
//==============================================================================
class SimTK_SIMBODY_EXPORT ContactTracker::HalfSpaceTriangleMesh
:   public ContactTracker {
public:
HalfSpaceTriangleMesh() 
:   ContactTracker(ContactGeometry::HalfSpace::classTypeId(),
                   ContactGeometry::TriangleMesh::classTypeId()) {}

virtual ~HalfSpaceTriangleMesh() {}

virtual bool trackContact
   (const Contact&         priorStatus,
    const Transform& X_GS1, 
    const ContactGeometry& surface1,    // the half space
    const Transform& X_GS2, 
    const ContactGeometry& surface2,    // the mesh
    Real                   cutoff,
    Contact&               currentStatus) const;

virtual bool predictContact
   (const Contact&         priorStatus,
    const Transform& X_GS1, const SpatialVec& V_GS1, const SpatialVec& A_GS1,
    const ContactGeometry& surface1,
    const Transform& X_GS2, const SpatialVec& V_GS2, const SpatialVec& A_GS2,
    const ContactGeometry& surface2,
    Real                   cutoff,
    Real                   intervalOfInterest,
    Contact&               predictedStatus) const;

virtual bool initializeContact
   (const Transform& X_GS1, const SpatialVec& V_GS1,
    const ContactGeometry& surface1,
    const Transform& X_GS2, const SpatialVec& V_GS2,
    const ContactGeometry& surface2,
    Real                   cutoff,
    Real                   intervalOfInterest,
    Contact&               contactStatus) const;

private:
void processBox(const ContactGeometry::TriangleMesh&              mesh, 
                const ContactGeometry::TriangleMesh::OBBTreeNode& node, 
                const Transform& X_HM, const UnitVec3& hsNormal_M, 
                Real hsFaceHeight_M, std::set<int>& insideFaces) const;
void addAllTriangles(const ContactGeometry::TriangleMesh::OBBTreeNode& node, 
                     std::set<int>& insideFaces) const; 
};



//==============================================================================
//                 SPHERE - TRIANGLE MESH CONTACT TRACKER
//==============================================================================
class SimTK_SIMBODY_EXPORT ContactTracker::SphereTriangleMesh
:   public ContactTracker {
public:
SphereTriangleMesh() 
:   ContactTracker(ContactGeometry::Sphere::classTypeId(),
                   ContactGeometry::TriangleMesh::classTypeId()) {}

virtual ~SphereTriangleMesh() {}

virtual bool trackContact
   (const Contact&         priorStatus,
    const Transform& X_GS1, 
    const ContactGeometry& surface1,    // the sphere
    const Transform& X_GS2, 
    const ContactGeometry& surface2,    // the mesh
    Real                   cutoff,
    Contact&               currentStatus) const;

virtual bool predictContact
   (const Contact&         priorStatus,
    const Transform& X_GS1, const SpatialVec& V_GS1, const SpatialVec& A_GS1,
    const ContactGeometry& surface1,
    const Transform& X_GS2, const SpatialVec& V_GS2, const SpatialVec& A_GS2,
    const ContactGeometry& surface2,
    Real                   cutoff,
    Real                   intervalOfInterest,
    Contact&               predictedStatus) const;

virtual bool initializeContact
   (const Transform& X_GS1, const SpatialVec& V_GS1,
    const ContactGeometry& surface1,
    const Transform& X_GS2, const SpatialVec& V_GS2,
    const ContactGeometry& surface2,
    Real                   cutoff,
    Real                   intervalOfInterest,
    Contact&               contactStatus) const;

private:
void processBox
   (const ContactGeometry::TriangleMesh&              mesh, 
    const ContactGeometry::TriangleMesh::OBBTreeNode& node, 
    const Vec3& center_M, Real radius2,   
    std::set<int>& insideFaces) const ;
};



//==============================================================================
//             TRIANGLE MESH - TRIANGLE MESH CONTACT TRACKER
//==============================================================================
class SimTK_SIMBODY_EXPORT ContactTracker::TriangleMeshTriangleMesh
:   public ContactTracker {
public:
TriangleMeshTriangleMesh() 
:   ContactTracker(ContactGeometry::TriangleMesh::classTypeId(),
                   ContactGeometry::TriangleMesh::classTypeId()) {}

virtual ~TriangleMeshTriangleMesh() {}

virtual bool trackContact
   (const Contact&         priorStatus,
    const Transform& X_GS1, 
    const ContactGeometry& surface1,    // mesh1
    const Transform& X_GS2, 
    const ContactGeometry& surface2,    // mesh2
    Real                   cutoff,
    Contact&               currentStatus) const;

virtual bool predictContact
   (const Contact&         priorStatus,
    const Transform& X_GS1, const SpatialVec& V_GS1, const SpatialVec& A_GS1,
    const ContactGeometry& surface1,
    const Transform& X_GS2, const SpatialVec& V_GS2, const SpatialVec& A_GS2,
    const ContactGeometry& surface2,
    Real                   cutoff,
    Real                   intervalOfInterest,
    Contact&               predictedStatus) const;

virtual bool initializeContact
   (const Transform& X_GS1, const SpatialVec& V_GS1,
    const ContactGeometry& surface1,
    const Transform& X_GS2, const SpatialVec& V_GS2,
    const ContactGeometry& surface2,
    Real                   cutoff,
    Real                   intervalOfInterest,
    Contact&               contactStatus) const;

private:
void findIntersectingFaces
   (const ContactGeometry::TriangleMesh&                mesh1, 
    const ContactGeometry::TriangleMesh&                mesh2,
    const ContactGeometry::TriangleMesh::OBBTreeNode&   node1, 
    const ContactGeometry::TriangleMesh::OBBTreeNode&   node2, 
    const OrientedBoundingBox&                          node2Bounds_M1,
    const Transform&                                    X_M1M2, 
    std::set<int>&                                      insideFaces1, 
    std::set<int>&                                      insideFaces2) const; 
void findBuriedFaces
   (const ContactGeometry::TriangleMesh&    mesh,
    const ContactGeometry::TriangleMesh&    otherMesh,
    const Transform&                        X_OM, 
    std::set<int>&                          insideFaces) const;
void tagFaces(const ContactGeometry::TriangleMesh&   mesh, 
              Array_<int>&                           faceType,
              std::set<int>&                         triangles, 
              int                                    index,
              int                                    depth) const;
};

} // namespace SimTK

#endif // SimTK_SIMBODY_CONTACT_TRACKER_SUBSYSTEM_H_