Utils.h

#ifndef CONTACT_UTILS_H
#define CONTACT_UTILS_H

#include <KrisLibrary/robotics/Contact.h>
#include <KrisLibrary/robotics/RobotWithGeometry.h>
#include "Stance.h"

namespace Klampt {

class WorldModel;

void GetFlatContacts(RobotWithGeometry& robot,Real tol,ContactFormation& contacts);


void GetFlatContacts(RobotWithGeometry& robot,int link,Real tol,vector<ContactPoint>& contacts);


void GetFlatStance(RobotWithGeometry& robot,Real tol,Stance& s,Real kFriction=0);


void GetNearbyContacts(RobotWithGeometry& robot,WorldModel& world,Real tol,ContactFormation& contacts);


void GetNearbyContacts(RobotWithGeometry& robot,int link,WorldModel& world,Real tol,vector<ContactPoint>& contacts);


void LocalContactsToHold(const vector<ContactPoint>& contacts,int link,const RobotKinematics3D& robot,Hold& hold);

void LocalContactsToStance(const ContactFormation& contacts,const RobotKinematics3D& robot,Stance& stance);

void ClusterContacts(vector<ContactPoint>& cps,int numClusters,Real clusterNormalScale=0.1,Real clusterFrictionScale=0.1);

void CleanupContacts(vector<ContactPoint>& cp,Real tol);

void CHContacts(vector<ContactPoint>& cp,Real ntol,Real xtol);

int ClosestContact(const ContactPoint& p,const Meshing::TriMesh& mesh,ContactPoint& closest,Real normalScale=0.1);

} //namespace Klampt

#endif