Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00014 #ifndef __SDCONTACT_H__
00015 #define __SDCONTACT_H__
00016
00017
00018 #include "chain.h"
00019 #include <vector>
00020
00021 #define SD_NONLINEAR
00022
00023 class SDContactPair
00024 {
00025 public:
00026 SDContactPair(Joint* _jnt1, Joint* _jnt2,
00027 double _spring = 1e5, double _damper = 10.0,
00028 double _static_fric = 0.0, double _slip_fric = 0.0,
00029 double _slip_p = 2000.0, double _slip_d = 700.0,
00030 double _slip_func_coef_base = 0.1) {
00031 joints[0] = _jnt1;
00032 joints[1] = _jnt2;
00033 spring = _spring;
00034 damper = _damper;
00035 static_fric = _static_fric;
00036 slip_fric = _slip_fric;
00037 slip_p = _slip_p;
00038 slip_d = _slip_d;
00039 slip_func_coef_base = _slip_func_coef_base;
00040 in_slip = false;
00041 }
00042 ~SDContactPair() {
00043 }
00044
00045 void SetSpring(double _spring) {
00046 spring = _spring;
00047 }
00048 void SetDamper(double _damper) {
00049 damper = _damper;
00050 }
00051 void SetStaticFric(double _static_fric) {
00052 static_fric = _static_fric;
00053 }
00054 void SetSlipFric(double _slip_fric) {
00055 slip_fric = _slip_fric;
00056 }
00057 double StaticFric() {
00058 return static_fric;
00059 }
00060 double SlipFric() {
00061 return slip_fric;
00062 }
00063
00064 Joint* GetJoint(int index) {
00065 return joints[index];
00066 }
00067
00068 void Clear() {
00069 coords.clear();
00070 normals.clear();
00071 depths.clear();
00072 }
00073
00074 void AddPoint(double* coord, double* normal, double depth) {
00075 coords.push_back(fVec3(coord[0], coord[1], coord[2]));
00076 normals.push_back(fVec3(normal[0], normal[1], normal[2]));
00077 depths.push_back(depth);
00078 }
00079
00080 int NumPoints() {
00081 return coords.size();
00082 }
00083 const fVec3& Coord(int index) {
00084 return coords[index];
00085 }
00086 const fVec3& Normal(int index) {
00087 return normals[index];
00088 }
00089 double Depth(int index) {
00090 return depths[index];
00091 }
00092
00093
00094 int Update(double timestep, int n_contact, double** coords, double** normals, double* depths);
00095
00096 protected:
00097 int set_init();
00098 int update(double timestep, int n_contact, double* coord, double* normal, double depth);
00099
00100
00101 Joint* joints[2];
00102
00103 std::vector<fVec3> coords;
00104 std::vector<fVec3> normals;
00105 std::vector<double> depths;
00106
00107
00108 double spring;
00109 double damper;
00110
00111 double static_fric;
00112 double slip_fric;
00113 double slip_p;
00114 double slip_d;
00115 double slip_func_coef_base;
00116
00117 int init_set;
00118 fVec3 init_pos;
00119 fMat33 init_att;
00120
00121 int in_slip;
00122 };
00123
00124 #endif