ContactFeature.h

Go to the documentation of this file.

#ifndef CONTACT_FEATURE_H
#define CONTACT_FEATURE_H

#include "Hold.h"
#include <KrisLibrary/math3d/Polygon3D.h>
#include <KrisLibrary/math3d/geometry3d.h>
#include <memory>

namespace Klampt {

struct Triangle3DSampler;


struct ContactFeatureBase
{
  enum Type { Point, Edge, Face, MultipleFaces, Wheel };

  ContactFeatureBase() :link(-1),weight(1) {}
  virtual ~ContactFeatureBase() {}
  virtual Type GetType() const =0;

  int link;
  string name;
  Real weight;   
};

struct PointContactFeature : public ContactFeatureBase
{
  virtual Type GetType() const { return Point; }

  void GetHold(const Vector3& x,Hold&) const;

  Vector3 p;
};

struct EdgeContactFeature : public ContactFeatureBase
{
  virtual Type GetType() const { return Edge; }

  void GetHold(const Vector3& x,const Vector3& axis,Hold&) const;

  void GetHold(const Vector3& x,const Vector3& n,Real theta,Hold&) const;

  Vector3 p1,p2;
};

struct FaceContactFeature : public ContactFeatureBase, Polygon3D
{
  virtual Type GetType() const { return Face; }

  void GetPlane(Plane3D& p) const;

  void GetCentroid(Vector3& p) const;

  void GetHold(const Vector3& x,const Vector3& n,Real theta,Hold&) const;
  void GetHold(const Vector3& localPos,const Vector3& x,const Vector3& n,Real theta,Hold&) const;
};

struct WheelContactFeature : public ContactFeatureBase
{
  virtual Type GetType() const { return Wheel; }

  Vector3 GetCenter() const { Vector3 center; axis.closestPoint(p,center); return center; }
  Real GetRadius() const { return axis.distance(p); }

  void GetHold(const Vector3& x,const Vector3& n,Real theta,Hold&) const;

  void GetFixedHold(const Vector3& x,const Vector3& n,Real theta,Real roll,Hold&) const;

  Line3D axis;
  Vector3 p;
  Real width;   //wheel's extent along the axis 
};

struct FacesContactFeature : public ContactFeatureBase
{
  virtual Type GetType() const { return MultipleFaces; }

  //TODO: get the holds

  vector<Polygon3D> faces;
};

typedef std::shared_ptr<ContactFeatureBase> ContactFeature;

bool LoadContactFeatures(const char* fn,vector<ContactFeature>& ccs);
bool SaveContactFeatures(const char* fn,const vector<ContactFeature>& ccs);

void SampleHold(const Vector3& x,const Vector3& n,const ContactFeatureBase*,Hold&);
void SampleHold(const Vector3& x,const Vector3& n,const vector<ContactFeature>&,Hold&);
void SampleHold(Triangle3DSampler& smp,const ContactFeatureBase*,Hold&);
void SampleHold(Triangle3DSampler& smp,const vector<ContactFeature>&,Hold&);

void HoldFromTransform(const ContactFeatureBase* f,const RigidTransform& T,Hold& h);

bool FeatureRayIntersection(const ContactFeatureBase* f,const Ray3D& ray,Real tol=0);

Vector3 FeatureContactPoint(const ContactFeatureBase* f);
Vector3 FeatureContactNormal(const ContactFeatureBase* f);
Vector3 SampleFeatureContactPoint(const ContactFeatureBase* f);

} //namespace Klampt

#endif