Contact.h

#ifndef CONTACT_H
#define CONTACT_H

#include <KrisLibrary/math3d/primitives.h>
#include <KrisLibrary/math/matrix.h>
#include <KrisLibrary/math/sparsematrix.h>
#include <vector>
#include <KrisLibrary/errors.h>
using namespace Math3D;

struct ContactPoint
{
  inline Real frictionConeHalfAngle() const { return Atan(kFriction); }
  inline void setfrictionConeHalfAngle(Real phi) { kFriction=Tan(phi); }
  bool isValidForce(const Vector3& f) const;
  Real minFriction(const Vector3& f) const;
  void quadraticForm(Matrix3& A,Vector3& b,Real& c) const;

  Vector3 x;
  Vector3 n;
  Real kFriction;
};

struct ContactPoint2D
{
  inline Real frictionConeHalfAngle() const { return Atan(kFriction); }
  inline void setfrictionConeHalfAngle(Real phi) { kFriction=Tan(phi); }
  bool isValidForce(const Vector2& f) const;
  Real minFriction(const Vector2& f) const;
  void quadraticForm(Matrix2& A,Vector2& b,Real& c) const;

  Vector2 x;
  Vector2 n;
  Real kFriction;
};

struct ContactFormation
{
  void set(int link,const std::vector<ContactPoint>& contacts,int target=-1);
  void concat(const ContactFormation& formation);
  void flatten(std::vector<int>& flatLinks,std::vector<ContactPoint>& cps) const;
  void flatten(std::vector<int>& flatLinks,std::vector<ContactPoint>& cps,std::vector<int>& flattargets) const;
  int numContactPoints() const;
  int numForceVariables() const;

  std::vector<int> links;
  std::vector<std::vector<ContactPoint> > contacts;
  std::vector<int> targets;
};

struct FrictionConePolygon
{
        void set(int k,const Vector3& n,Real kFriction);
        bool contains(const Vector3& f) const;
        bool onBoundary(const Vector3& f) const;

        std::vector<Vector3> edges;
        std::vector<Vector3> planes;
};

void FrictionToFrictionlessContacts(const std::vector<ContactPoint>& c1,int k,std::vector<ContactPoint>& c2);

void FrictionToFrictionlessContacts(const std::vector<ContactPoint2D>& c1,std::vector<ContactPoint2D>& c2);

void GetFrictionConePlanes(const ContactPoint& contact,int k,Matrix& A);

void GetFrictionConePlanes(const ContactPoint2D& contact,Matrix2& A);

void GetWrenchMatrix(const std::vector<ContactPoint>& s,const Vector3& cm,Matrix& A);
void GetWrenchMatrix(const std::vector<ContactPoint>& s,const Vector3& cm,SparseMatrix& A);
void GetWrenchMatrix(const ContactFormation& s,const Vector3& cm,SparseMatrix& A);

void GetFrictionConePlanes(const std::vector<ContactPoint>& contacts,int k,Matrix& A);
void GetFrictionConePlanes(const std::vector<ContactPoint>& contacts,int k,SparseMatrix& A);
void GetFrictionConePlanes(const ContactFormation& s,int k,SparseMatrix& A);

void GetFrictionConePlanes(const std::vector<ContactPoint2D>& contacts,Matrix& A);
void GetFrictionConePlanes(const std::vector<ContactPoint2D>& contacts,SparseMatrix& A);





struct CustomContactPoint
{
  CustomContactPoint();
  CustomContactPoint(const ContactPoint& cp,int numFCEdges=4);
  void set(const ContactPoint& cp,int numFCEdges=4);
  void setRobustnessFactor(Real offset);
  void addNormalForceBounds(Real minimum,Real maximum);
  void calculateForceMatrix(int numFCEdges=4);
  void calculateWrenchMatrix(int numFCEdges=4);
  int numForceVariables() const;
  int numConstraints() const;

  Vector3 x;
  Vector3 n;
  Real kFriction;
  Matrix forceMatrix;
  Vector forceOffset;
  Matrix wrenchMatrix;
  Vector wrenchOffset;
};

struct CustomContactPoint2D
{
  CustomContactPoint2D();
  CustomContactPoint2D(const ContactPoint2D& cp);
  void set(const ContactPoint2D& cp);
  void setRobustnessFactor(Real offset);
  void addNormalForceBounds(Real minimum,Real maximum);
  void calculateForceMatrix();
  void calculateWrenchMatrix();
  int numForceVariables() const;
  int numConstraints() const;

  Vector2 x;
  Vector2 n;
  Real kFriction;
  Matrix forceMatrix;
  Vector forceOffset;
  Matrix wrenchMatrix;
  Vector wrenchOffset;
};

struct CustomContactFormation
{
  void clear();
  void set(int link,const std::vector<ContactPoint>& contacts,int numFCEdges);
  void set(int link,const std::vector<CustomContactPoint>& contacts);
  void set(const ContactFormation& formation,int numFCEdges);
  void concat(const CustomContactFormation& formation);
  void addLinkForceLimit(int link,const Vector3& direction,Real maximum);
  void addLinkWrenchLimit(int link,const Vector3& fdirection,const Vector3& mdirection,Real maximum);
  void addForceLimit(const std::vector<int>& contacts,const Vector3& direction,Real maximum);
  void addWrenchLimit(const std::vector<int>& contacts,const Vector3& fdirection,const Vector3& mdirection,Real maximum);
  void addLinkForceConstraint(int link,const Matrix& A,const Vector& b,bool equality=false);
  void addLinkWrenchConstraint(int link,const Matrix& A,const Vector& b,bool equality=false);
  void addForceConstraint(const std::vector<int>& contacts,const Matrix& A,const Vector& b,bool equality=false);
  void addForceConstraint(const std::vector<int>& contacts,const std::vector<Matrix>& A,const Vector& b,bool equality=false);
  void addWrenchConstraint(const std::vector<int>& contacts,const Matrix& A,const Vector& b,bool equality=false);
  void addWrenchConstraint(const std::vector<int>& contacts,const std::vector<Matrix>& A,const Vector& b,bool equality=false);
  int numForceVariables() const;
  int numConstraints() const;
 
  std::vector<int> links;
  std::vector<CustomContactPoint> contacts;
  std::vector<int> targets;
  std::vector<std::vector<int> > constraintGroups;
  std::vector<std::vector<Matrix> > constraintMatrices;
  std::vector<Vector> constraintOffsets;
  std::vector<bool> constraintEqualities;
};

void GetForceMatrix(const std::vector<CustomContactPoint>& contacts,SparseMatrix& A);
void GetForceMatrix(const CustomContactFormation& contacts,SparseMatrix& A);


void GetWrenchMatrix(const std::vector<CustomContactPoint>& contacts,const Vector3& cm,Matrix& A);
void GetWrenchMatrix(const std::vector<CustomContactPoint>& contacts,const Vector3& cm,SparseMatrix& A);
void GetWrenchMatrix(const CustomContactFormation& contacts,const Vector3& cm,SparseMatrix& A);


void GetFrictionConePlanes(const std::vector<CustomContactPoint>& contacts,Matrix& A,Vector& b);
void GetFrictionConePlanes(const std::vector<CustomContactPoint>& contacts,SparseMatrix& A,Vector& b);
void GetFrictionConePlanes(const CustomContactFormation& s,SparseMatrix& A,Vector& b);

void GetFrictionConePlanes(const std::vector<CustomContactPoint2D>& contacts,Matrix& A,Vector& b);
void GetFrictionConePlanes(const std::vector<CustomContactPoint2D>& contacts,SparseMatrix& A,Vector& b);



#endif