MBSim

Project Page
contact_utils.h
00001 /* Copyright (C) 2004-2009 MBSim Development Team
00002  *
00003  * This library is free software; you can redistribute it and/or 
00004  * modify it under the terms of the GNU Lesser General Public 
00005  * License as published by the Free Software Foundation; either 
00006  * version 2.1 of the License, or (at your option) any later version. 
00007  *  
00008  * This library is distributed in the hope that it will be useful, 
00009  * but WITHOUT ANY WARRANTY; without even the implied warranty of 
00010  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU 
00011  * Lesser General Public License for more details. 
00012  *  
00013  * You should have received a copy of the GNU Lesser General Public 
00014  * License along with this library; if not, write to the Free Software 
00015  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301 USA
00016  *
00017  * Contact: martin.o.foerg@googlemail.com
00018  *          rzander@users.berlios.de
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++) { // loop with respect to contours
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++) { // use all points
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 /* _CONTACT_UTILS_H_ */
00106