ContactSurface.h

Go to the documentation of this file.

#ifndef SimTK_SIMBODY_CONTACT_SURFACE_H_
#define SimTK_SIMBODY_CONTACT_SURFACE_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) 2008-13 Stanford University and the Authors.        *
 * Authors: Peter Eastman, Michael Sherman                                    *
 * 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 <algorithm>
#include <atomic>

namespace SimTK {

class ContactGeometry;

SimTK_DEFINE_UNIQUE_INDEX_TYPE(ContactCliqueId);



//==============================================================================
//                               CONTACT MATERIAL
//==============================================================================
class SimTK_SIMBODY_EXPORT ContactMaterial {
public:
ContactMaterial() {clear();}

ContactMaterial(Real stiffness, Real dissipation, 
                Real staticFriction, Real dynamicFriction,
                Real viscousFriction = 0) {
    setStiffness(stiffness);
    setDissipation(dissipation);
    setFriction(staticFriction, dynamicFriction, viscousFriction);
}

bool isValid() const {return m_stiffness > 0;}

Real getStiffness() const 
{   SimTK_ERRCHK(isValid(), "ContactMaterial::getStiffness()",
        "This is an invalid ContactMaterial.");
    return m_stiffness; }
Real getStiffness23() const 
{   SimTK_ERRCHK(isValid(), "ContactMaterial::getStiffness23()",
        "This is an invalid ContactMaterial.");
    return m_stiffness23; }
Real getDissipation() const 
{   SimTK_ERRCHK(isValid(), "ContactMaterial::getDissipation()",
        "This is an invalid ContactMaterial.");
    return m_dissipation; }
Real getStaticFriction() const 
{   SimTK_ERRCHK(isValid(), "ContactMaterial::getStaticFriction()",
        "This is an invalid ContactMaterial.");
    return m_staticFriction; }
Real getDynamicFriction() const 
{   SimTK_ERRCHK(isValid(), "ContactMaterial::getDynamicFriction()",
        "This is an invalid ContactMaterial.");
    return m_dynamicFriction; }
Real getViscousFriction() const 
{   SimTK_ERRCHK(isValid(), "ContactMaterial::getViscousFriction()",
        "This is an invalid ContactMaterial.");
    return m_viscousFriction; }

ContactMaterial& setStiffness(Real stiffness) {
    SimTK_ERRCHK1_ALWAYS(stiffness >= 0, "ContactMaterial::setStiffness()",
        "Stiffness %g is illegal; must be >= 0.", stiffness);
    m_stiffness = stiffness;
    m_stiffness23 = std::pow(m_stiffness, Real(2./3.));
    return *this;
}

ContactMaterial& setDissipation(Real dissipation) {
    SimTK_ERRCHK1_ALWAYS(dissipation >= 0, "ContactMaterial::setDissipation()",
        "Dissipation %g (in 1/speed) is illegal; must be >= 0.", dissipation);
    m_dissipation = dissipation;
    return *this;
}

ContactMaterial& setFriction(Real staticFriction,
                             Real dynamicFriction,
                             Real viscousFriction = 0)
{
    SimTK_ERRCHK1_ALWAYS(0 <= staticFriction, "ContactMaterial::setFriction()",
        "Illegal static friction coefficient %g.", staticFriction);
    SimTK_ERRCHK2_ALWAYS(0<=dynamicFriction && dynamicFriction<=staticFriction, 
        "ContactMaterial::setFriction()",
        "Dynamic coefficient %g illegal; must be between 0 and static"
        " coefficient %g.", dynamicFriction, staticFriction);
    SimTK_ERRCHK1_ALWAYS(0 <= viscousFriction, "ContactMaterial::setFriction()",
        "Illegal viscous friction coefficient %g.", viscousFriction);

    m_staticFriction  = staticFriction;
    m_dynamicFriction = dynamicFriction;
    m_viscousFriction = viscousFriction;
    return *this;
}

static Real calcPlaneStrainStiffness(Real youngsModulus, 
                                     Real poissonsRatio) 
{
    SimTK_ERRCHK2_ALWAYS(youngsModulus >= 0 &&
                         -1 < poissonsRatio && poissonsRatio <= 0.5,
                         "ContactMaterial::calcStiffnessForSolid()",
        "Illegal material properties E=%g, v=%g.", 
        youngsModulus, poissonsRatio);

    return youngsModulus / (1-square(poissonsRatio)); 
}


static Real calcConfinedCompressionStiffness(Real youngsModulus, 
                                             Real poissonsRatio) 
{
    SimTK_ERRCHK2_ALWAYS(youngsModulus >= 0 &&
                         -1 < poissonsRatio && poissonsRatio < 0.5,
                         "ContactMaterial::calcStiffnessForSolid()",
        "Illegal material properties E=%g, v=%g.", 
        youngsModulus, poissonsRatio);

    return youngsModulus*(1-poissonsRatio) 
        / ((1+poissonsRatio)*(1-2*poissonsRatio)); 
}

static Real calcDissipationFromObservedRestitution
   (Real restitution, Real speed) {
    if (restitution==1) return 0;
    SimTK_ERRCHK2_ALWAYS(0<=restitution && restitution<=1 && speed>0,
        "ContactMaterial::calcDissipationFromRestitution()",
        "Illegal coefficient of restitution or speed (%g,%g).",
        restitution, speed);
    return (1-restitution)/speed;
}

void clear() {
    m_stiffness   = NaN; // unspecified
    m_stiffness23 = NaN;
    m_restitution = NaN; // unspecified
    m_dissipation = 0;  // default; no dissipation
    // default; no friction
    m_staticFriction=m_dynamicFriction=m_viscousFriction = 0;
}

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

// For compliant contact models.
Real    m_stiffness;        // k: stress/%strain=(force/area)/%strain
Real    m_stiffness23;      // k^(2/3) in case we need it
Real    m_dissipation;      // c: %normalForce/normalVelocity

// For impulsive collisions.
Real    m_restitution;      // e: unitless, e=(1-cv)

// Friction.
Real    m_staticFriction;   // us: unitless
Real    m_dynamicFriction;  // ud: unitless
Real    m_viscousFriction;  // uv: %normalForce/slipVelocity
};



//==============================================================================
//                             CONTACT SURFACE
//==============================================================================
class SimTK_SIMBODY_EXPORT ContactSurface {
public:
ContactSurface() : m_thickness(0), m_indexOnBody(-1), m_userRef(nullptr) {}
ContactSurface(const ContactGeometry&   shape, 
               const ContactMaterial&   material,
               Real                     thickness=0)
:   m_shape(shape), m_material(material), m_thickness(thickness),
    m_indexOnBody(-1), m_userRef(nullptr) {
    SimTK_ERRCHK1_ALWAYS(thickness >= 0, "ContactSurface::ctor()",
        "Illegal thickness %g.", thickness);
}

ContactSurface& setShape(const ContactGeometry& shape) 
{   m_shape = shape; return *this; }

ContactSurface& setMaterial(const ContactMaterial& material,
                            Real thickness = 0) 
{   m_material = material; setThickness(thickness); return *this; }

ContactSurface& setThickness(Real thickness) {
    SimTK_ERRCHK1_ALWAYS(thickness >= 0, "ContactSurface::setThickness()",
        "Illegal thickness %g.", thickness);
    m_thickness = thickness; 
    return *this; 
}

const ContactGeometry& getShape()    const {return m_shape;}

const ContactMaterial& getMaterial() const {return m_material;}

Real getThickness() const {return m_thickness;}

ContactGeometry& updShape()    {return m_shape;}

ContactMaterial& updMaterial() {return m_material;}

ContactSurface& joinClique(ContactCliqueId clique) {
    if (!clique.isValid()) return *this;
    // Although this is a sorted list, we expect it to be very short so 
    // are using linear search to find where this new clique goes.
    Array_<ContactCliqueId,short>::iterator p;
    for (p = m_cliques.begin(); p != m_cliques.end(); ++p) {   
        if (*p==clique) return *this; // already a member
        if (*p>clique) break;
    }
    // insert just before p (might be at the end)
    m_cliques.insert(p, clique);
    return *this;
}

void leaveClique(ContactCliqueId clique) {   
    if (!clique.isValid()) return;
    // We expect this to be a very short list so are using linear search.
    Array_<ContactCliqueId,short>::iterator p =
        std::find(m_cliques.begin(), m_cliques.end(), clique);
    if (p != m_cliques.end()) m_cliques.erase(p); 
}

bool isInSameClique(const ContactSurface& other) const 
{   if (getCliques().empty() || other.getCliques().empty()) return false;//typical
    return cliquesIntersect(getCliques(), other.getCliques()); }

const Array_<ContactCliqueId,short>& getCliques() const {return m_cliques;}

static bool cliquesIntersect(const Array_<ContactCliqueId,short>& a,
                             const Array_<ContactCliqueId,short>& b)
{
    Array_<ContactCliqueId,short>::const_iterator ap = a.begin();
    Array_<ContactCliqueId,short>::const_iterator bp = b.begin();
    // Quick checks: empty or non-overlapping.
    if (ap==a.end() || bp==b.end()) return false;
    if (*ap > b.back() || *bp > a.back()) 
        return false; // disjoint
    // Both lists have elements and the elements overlap numerically.
    while (true) {
        if (*ap==*bp) return true;
        // Increment the list with smaller front element.
        if (*ap < *bp) {++ap; if (ap==a.end()) break;}
        else           {++bp; if (bp==b.end()) break;}
    }
    // One of the lists ran out of elements before we found a match.
    return false;
}

static ContactCliqueId createNewContactClique()
{   static std::atomic<int> nextAvailableContactClique(1);
    return ContactCliqueId(nextAvailableContactClique++); }

ContactSurface& setIndexOnBody(int index) {m_indexOnBody=index; return *this;}

ContactSurface& setUserRef(void* userRef) {m_userRef=userRef; return *this;}

int getIndexOnBody() const {return m_indexOnBody;}

void* getUserRef() const {return m_userRef;} 

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

ContactGeometry                 m_shape;
ContactMaterial                 m_material;
Real                            m_thickness;    // default=0; meaning not set
Array_<ContactCliqueId,short>   m_cliques;      // sorted

int                             m_indexOnBody;  // default is -1
void*                           m_userRef;      // default is nullptr
};



} // namespace SimTK

#endif // SimTK_SIMBODY_CONTACT_SURFACE_H_