mbsimflexiblebody  3.2.0
functions_contact.h
00001 /* Copyright (C) 2004-2011 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: thorsten.schindler@mytum.de
00018  */
00019 
00020 #ifndef MBSIMFLEXIBLEBODY_FUNCTIONS_CONTACT_H_
00021 #define MBSIMFLEXIBLEBODY_FUNCTIONS_CONTACT_H_
00022 
00023 #include "mbsim/contours/circle.h"
00024 #include "mbsimFlexibleBody/contours/nurbs_disk_2s.h"
00025 //#include "mbsim/utils/function.h"
00026 #include "mbsim/functions_contact.h"
00027 
00028 namespace MBSimFlexibleBody {
00029 
00035   class FuncPairCircleNurbsDisk2s : public MBSim::DistanceFunction<double,double> {
00036     public:
00042       FuncPairCircleNurbsDisk2s(MBSim::Circle* circle_, NurbsDisk2s* nurbsdisk_) : nurbsdisk(nurbsdisk_), circle(circle_) {}
00043 
00044       /* INHERITED INTERFACE OF DISTANCEFUNCTION */
00045       double operator()(const double &alpha, const void * =NULL) {
00046         //Parameters of the AWK of the nurbs disk and the circle
00047         fmatvec::SqrMat3 AWK_disk   = nurbsdisk->getFrame()->getOrientation();
00048         fmatvec::SqrMat3 AWK_circle = circle->getFrame()->getOrientation();
00049 
00050         //Point on the Circle
00051         fmatvec::Vec3 WP_circle(fmatvec::INIT,0.);  //world-coordinates of the point on the circle
00052         WP_circle(0) = cos(alpha);  
00053         WP_circle(1) = sin(alpha);
00054         WP_circle = circle->getFrame()->getPosition() + circle->getRadius() * AWK_circle * WP_circle;
00055 
00056         //derivatives of a point on the circle in world-coordinates with respect to the circle-parameter alpha
00057         fmatvec::Vec dWP_circle(3,fmatvec::INIT, 0.);
00058         dWP_circle(0) = -sin(alpha);  
00059         dWP_circle(1) = cos(alpha);
00060         fmatvec::Vec circle_tangent = circle->getRadius() * AWK_circle * dWP_circle; //not normalised tangent on the circle
00061 
00062         //compute radial and azimuthal nurbsdisk-coordinates out of alpha (saved in the LagrangeParameterPosition)
00063         MBSim::ContourPointData cp_nurbsdisk;
00064         cp_nurbsdisk.getLagrangeParameterPosition() = nurbsdisk->transformCW( AWK_disk.T() * (WP_circle - nurbsdisk->getFrame()->getPosition()) ); // position of the point in the cylinder-coordinates of the disk
00065 
00066         //get the position and the derivatives on the disk 
00067         nurbsdisk->updateKinematicsForFrame(cp_nurbsdisk,MBSim::firstTangent); 
00068 
00069         //compute the derivates of the radial and the azimuthal coordinates with respect to alpha
00070         fmatvec::SqrMat A_inv(3,fmatvec::EYE);
00071         A_inv(0,0)=  cos(cp_nurbsdisk.getLagrangeParameterPosition()(1)); 
00072         A_inv(0,1)=  sin(cp_nurbsdisk.getLagrangeParameterPosition()(1)); 
00073         A_inv(1,0)= -sin(cp_nurbsdisk.getLagrangeParameterPosition()(1)) / cp_nurbsdisk.getLagrangeParameterPosition()(0); 
00074         A_inv(1,1)=  cos(cp_nurbsdisk.getLagrangeParameterPosition()(1)) / cp_nurbsdisk.getLagrangeParameterPosition()(0);
00075         fmatvec::Vec drphidalpha = A_inv * AWK_disk.T()* circle_tangent; // AWK_disk * A_inv * trans(AWK_disk)* circle_tangent CHANGED
00076 
00077         //compution of the single elements in the function
00078         fmatvec::Vec nurbs_radial_tangent    = cp_nurbsdisk.getFrameOfReference().getOrientation().col(1);
00079         fmatvec::Vec nurbs_azimuthal_tangent = cp_nurbsdisk.getFrameOfReference().getOrientation().col(2);
00080 
00081         return nurbsdisk->getFrame()->getOrientation().col(2).T() * (circle_tangent - (nurbs_radial_tangent *  drphidalpha(0)+ nurbs_azimuthal_tangent * drphidalpha(1)));
00082       }
00083 
00084       fmatvec::Vec3 computeWrD(const double &alpha) {
00085         //point on the circle
00086         fmatvec::Vec WP_circle(3,fmatvec::INIT,0.);
00087         WP_circle(0) = cos(alpha); 
00088         WP_circle(1) = sin(alpha);
00089         WP_circle = circle->getFrame()->getPosition() + circle->getRadius() * circle->getFrame()->getOrientation() * WP_circle;    
00090 
00091         //get the position on the nurbsdisk
00092         MBSim::ContourPointData cp_nurbsdisk;
00093         cp_nurbsdisk.getLagrangeParameterPosition() = nurbsdisk->transformCW(nurbsdisk->getFrame()->getOrientation().T()*(WP_circle - nurbsdisk->getFrame()->getPosition())); // position of the point in the cylinder-coordinates of the disk
00094         nurbsdisk->updateKinematicsForFrame(cp_nurbsdisk,MBSim::position);
00095         fmatvec::Vec WP_nurbsdisk = cp_nurbsdisk.getFrameOfReference().getPosition();
00096 
00097         return WP_circle - WP_nurbsdisk;
00098       }
00099       /***************************************************/
00100 
00101     private:
00105       NurbsDisk2s *nurbsdisk;
00106       MBSim::Circle *circle;
00107   };
00108 
00109 }
00110 
00111 #endif /* MBSIMFLEXIBLEBODY_FUNCTIONS_CONTACT_H_ */
00112