00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021 #ifndef _CONTACT_UTILS_H_
00022 #define _CONTACT_UTILS_H_
00023
00024 #include "mbsim/contact.h"
00025 #include "mbsim/contour.h"
00026 #include "mbsim/contours/contour_interpolation.h"
00027 #include "mbsim/dynamic_system_solver.h"
00028
00029 #include <vector>
00030
00031 namespace MBSim {
00032
00038 double computeAngleOnUnitCircle(const fmatvec::Vec3& r);
00039
00045 fmatvec::Vec2 computeAnglesOnUnitSphere(const fmatvec::Vec3& r);
00046
00053 ContactKinematics* findContactPairingRigidRigid(const char* contour0, const char* contour1);
00054
00060 template <class T>
00061 void ContactContourInterpolation(DynamicSystemSolver *ds, T *contact, ContourInterpolation *contour0, ContourInterpolation *contour1) {
00062 ContactContourInterpolation(ds,contact,contour0,contour1,0);
00063 }
00064
00070 template <class T>
00071 void ContactContourInterpolation(DynamicSystemSolver *ds, const T *contact, ContourInterpolation *contour0, ContourInterpolation *contour1, int master) {
00072 ContourInterpolation *contour[2];
00073 contour[0] = contour0;
00074 contour[1] = contour1;
00075 std::string contactName = contact->getName();
00076
00077 int cStart, cEnd;
00078 switch(master) {
00079 case 0: cStart = 0; cEnd = 1; break;
00080 case 1: cStart = 1; cEnd = 1; break;
00081 case 2: cStart = 0; cEnd = 0; break;
00082 }
00083
00084 for(int c = cStart;c<cEnd+1;c++) {
00085 int numberOfPoints = contour[c]->getNPoints();
00086 char contourName;
00087 switch(c) {
00088 case 0: contourName = 'A';break;
00089 case 1: contourName = 'B';break;
00090 }
00091 for(int i = 0;i<numberOfPoints; i++) {
00092 std::stringstream number;
00093 std::string name = contactName + number.str();
00094
00095 T *newContact = new T( contact , name );
00096
00097 newContact->connect(contour[c]->getPoint(i),contour[1-c]);
00098 ds->addLink(newContact);
00099 }
00100 }
00101 }
00102
00103 }
00104
00105 #endif
00106