ContactTracker.h

Go to the documentation of this file.

#ifndef SimTK_SIMMATH_CONTACT_TRACKER_SUBSYSTEM_H_
#define SimTK_SIMMATH_CONTACT_TRACKER_SUBSYSTEM_H_

/* -------------------------------------------------------------------------- *
 *                        Simbody(tm): SimTKmath                              *
 * -------------------------------------------------------------------------- *
 * 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) 2010-12 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 "SimTKcommon.h"
#include "simmath/internal/common.h"
#include "simmath/internal/Contact.h"

namespace SimTK {

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

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;

static bool refineImplicitPair
   (const ContactGeometry& shapeA, Vec3& pointP_A,    // in/out
    const ContactGeometry& shapeB, Vec3& pointQ_B,    // in/out
    const Transform& X_AB, Real accuracyRequested,
    Real& accuracyAchieved, int& numIterations);

static Vec6 findImplicitPairError
   (const ContactGeometry& shapeA, const Vec3& pointP,
    const ContactGeometry& shapeB, const Vec3& pointQ,
    const Transform& X_AB);

static Mat66 calcImplicitPairJacobian
   (const ContactGeometry& shapeA, const Vec3& pointP,
    const ContactGeometry& shapeB, const Vec3& pointQ,
    const Transform& X_AB, const Vec6& err0);

static bool estimateConvexImplicitPairContactUsingMPR
   (const ContactGeometry& shapeA, const ContactGeometry& shapeB, 
    const Transform& X_AB,
    Vec3& pointP_A, Vec3& pointQ_B, UnitVec3& dirInA,
    int& numIterations);


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


//==============================================================================
//               CONVEX IMPLICIT SURFACE PAIR CONTACT TRACKER
//==============================================================================
class SimTK_SIMMATH_EXPORT ContactTracker::ConvexImplicitPair 
:   public ContactTracker {
public:
ConvexImplicitPair(ContactGeometryTypeId type1, ContactGeometryTypeId type2) 
:   ContactTracker(type1, type2) {}

virtual ~ConvexImplicitPair() {}

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


//==============================================================================
//                GENERAL IMPLICIT SURFACE PAIR CONTACT TRACKER
//==============================================================================
class SimTK_SIMMATH_EXPORT ContactTracker::GeneralImplicitPair 
:   public ContactTracker {
public:
GeneralImplicitPair(ContactGeometryTypeId type1, ContactGeometryTypeId type2) 
:   ContactTracker(type1, type2) {}

virtual ~GeneralImplicitPair() {}

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

} // namespace SimTK

#endif // SimTK_SIMMATH_CONTACT_TRACKER_SUBSYSTEM_H_